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ABSTRACT 


A  Fault  Detection  and  isolation  (FDI)  algorithm  design  is  presented  using  the 
Multiple  Model  algorithm  technique  for  the  Bluebird  aircraft  being  developed  at 
the  Naval  Postgraduate  School.  The  requirement  to  maintain  high  performance  in 
the  dynamic  system  of  the  aircraft  necessitates  the  use  of  FDI  techniques  to  detect 
and  isolate  malfunctions  in  the  sensors  and  actuators  of  the  aircraft  without  using 
hardware  redundancy.  The  solution  presented  makes  use  of  analytical  redundancy  in 
a  bank  of  Kalman  filters.  Statistical  tests  using  Bayesian  theory  are  applied  on  the 
filter’s  innovations  to  perform  the  task  of  detection  and  isolation.  The  algorithm  was 
developed  using  MATLAB  software  from  The  Math  Works.  Inc.  The  work  presented 
in  this  thesis  is  related  only  to  the  task  of  FDI.  The  remaining  task  of  the  monitoring 
system,  reconfiguration  and  continued  operation  by  the  observed  plant  after  a  failure 
detection,  will  not  be  addressed. 
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I.  INTRODUCTION  TO  FAULT  DETECTION 

AND  ISOLATION 


The  field  of  Fault  Detection  and  Isolation  (FDI)  has  evolved  as  a  result  of  ef¬ 
forts  to  produce  better  control  algorithms  for  dynamic  systems.  Continually  higher 
standards  of  performance,  reliability  and  survivability  dictate  the  development  of 
new  control  techniques  to  achieve  certain  hardware  goals.  Hardware  performance 
goals  can  be  related  to  lowering  noise,  eliminating  biases  and  meeting  bandwidth 
specifications  for  sensors  actuators  and  other  mechanical  components.  Reliability 
specifications  are  evolving  and  form  the  basis  for  determining  the  minimum  hardware 
and  software  complement  to  deal  with  performance  requirements.  An  example  of  a 
reliability  specification  is  that  which  applies  to  aircraft  and  dictates  the  probability 
of  catastrophic  failure  to  be  at  10~9  or  better  for  civilian  aircraft  or  at  10-4  to  10-5 
for  military  applications  [Ref.  1,  p.1-1].  The  survivability  criterion  applies  more  to 
military  applications  where  the  continuation  of  a  mission  is  important  after  the  sys¬ 
tem  has  been  partly  damaged.  FDI  techniques  can  be  applied  to  the  design  of  Flight 
Guidance  and  Control  Systems  to  increase  their  reliability,  lower  the  cost  associated 
with  hardware  redundancy,  and  improve  aircraft  survivability  by  allowing  more  dis¬ 
persion  of  components  [Ref.  1].  The  main  task  of  failure  detection  and  compensation 
is  to  modify  the  normal  mode  configuration  in  order  to  include  the  capability  of  de¬ 
tecting  abnormal  changes  and  compensating  for  them  by  activating  back-up  systems, 
adjusting  feedback  gains,  or  taking  other  correction  measures.  The  primary  func¬ 
tion  of  the  FDI  algorithm  itself  is  to  register  an  alarm  when  an  abnormal  condition 
develops  in  a  monitored  system  and  to  identify  the  component  at  the  source  of  the 
problem.  The  failure  detection  and  isolation  system  should  provide  the  capability 
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to  detect  the  occurrence  of  failures  in  a  given  system  and  isolate  the  faulty  sensors 
or  hardware,  all  in  the  presence  of  noise  and  errors  in  the  model.  The  basis  for  the 
decision  in  the  event  of  a  fault  is  the  fault  signature,  in  other  words,  a  signal  that  is 
obtained  from  some  kind  of  faulty  system  model  defining  the  effects  associated  with 
a  fault.  The  difficulty  associated  with  the  design  of  FDI  systems  lies  in  maintaining 
their  ability  to  detect  small  failures,  while  avoiding  false  alarms  in  the  presence  of 
noise  and  erroneous  models  which  cause  effects  similar  to  the  failures  signatures. 

A.  SUBDIVISIONS  OF  FDI 

FDI  systems  are  concerned  with  determining  the  details  of  a  particular  failure 
such  as  its  type,  its  source,  its  location  and  its  size. 

1.  Type  and  Size  of  Failure 

The  type  of  a  failure  can  be  divided  into  two  parts: 

•  Abrupt  or  hard  faults. 

•  Soft  or  incipient  faults. 

An  abrupt  fault  can  also  be  described  as  hard  or  catastrophic,  and  results  in  drastic 
changes  in  the  model.  On  the  other  hand,  an  incipient  or  soft  failure  is  characterized 
by  a  slowly  developing  and  time  varying  perturbation  in  the  system. 

2.  Sources  of  Failure 

The  sources  of  failures  in  Flight  Control  Systems  are  usually  described  by 
actuator  or  sensor  failures.  A  failed  sensor  can  be  detected  by  hardware  redundancy. 
This  method  is  implemented  by  using  several  sensors  to  measure  the  same  signal. 
The  outputs  can  be  compared  in  a  voting  scheme  in  order  to  detect  significant  differ¬ 
ences  between  the  repeated  sensors  and  isolate  a  faulty  sensor.  This  FDI  method  can 
become  very  costly  for  aircraft  using  hundreds  of  sensors,  and  this  hardware  multipli¬ 
cation  can  also  significantly  increase  the  overall  weight  of  the  aircraft.  The  additional 


2 


space  required  to  accommodate  the  equipment  also  becomes  a  major  concern  in  the 
already  limited  and  crowded  environment  inside  an  aircraft.  The  use  of  hardware 
redundancy  to  detect  erroneous  actuators  or  system  failures  is  usually  not  practical 
because  the  replication  of  components  other  than  sensors  is  not  feasible  [Ref.  2].  The 
problems  associated  with  hardware  redundancy  motivated  the  introduction  of  the 
concept  of  analytical  redundancy  in  FDI.  In  regards  to  present  aircraft,  the  onboard 
use  of  highly  efficient  computers  capable  of  mathematically  intensive  calculations, 
now  renders  possible  the  use  of  algorithms  to  perform  the  task  of  FDI.  The  analyti¬ 
cal  redundancy  approach  uses  the  system  model  instead  of  hardware  redundancy  to 
detect  and  isolate  failures  in  the  observed  system. 

3.  Location  of  Failures 

The  term  location,  identification  or  isolation  are  often  used  interchangeably 
in  FDI  when  determining  which  sensor  or  actuator  is  at  fault.  The  level  of  coupling 
in  the  system  is  a  criteria  to  consider  when  classifying  the  isolation  problem.  For 
example,  jet  engine  and  flight  control  systems  involve  plants  with  strong  coupling. 
Their  subsystems  are  also  strongly  coupled,  and  because  of  this,  usually  not  all  the 
state  variables  are  being  measured.  To  achieve  the  isolation  process  for  these  systems 
use  of  analytical  redundancy  is  necessary  [Ref.  2]. 

B.  FDI  VERSUS  BUILT  IN  TEST  TECHNIQUE 

FDI  techniques  differ  from  other  error  detection  schemes  implemented  by  means 
of  Built-in  Test  Equipments  (usually  called  BITE’s).  BITE  are  usually  designed  to 
test  for  hardware  malfunction  using  hardware  or  software  methods  such  as: 

•  CPU  tests  to  check  the  instruction  repertory. 

•  Parity  control  on  data  and  address  buses. 

•  Wrap  around  circuitries  used  to  test  two  way  interfaces  and  mainly  useful  at 
system  level  for  integration  purposes. 
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•  Power  sensing  circuitries  to  detect  when  the  applied  power  goes  out  of  the 
expected  range. 

•  RAM  tests  through  write-then-read  cycles  performed  on  all  the  memory  loca¬ 
tions  and  with  particular  patterns. 

•  ROM  tests  performed  by  checking  the  run  time  computed  checksums  against 
prestored  ones. 

•  Wrap  around  tests  that  require  data  management. 

•  Special-to-project  logic  checks  that  control  the  global  development  of  the  system 
versus  time.  [Ref.  3,  4] 

These  tests  are  implemented  in  self-monitored  systems  and  run  in  external  self- 
contained  units.  BITE’s  often  make  use  of  the  concept  of  graceful  degradation  when 
the  loss  of  redundancy  would  result  in  a  penalization  that  would  be  considered  too 
high.  For  some  failures,  specified  degraded  modes  of  operation  are  better  suited.  This 
approach  is  generally  dealt  with  by  specifying  for  each  important  function  a  nominal 
operation  and  a  reduced  level  of  operation.  The  BITE  approach  is  more  limited  than 
the  FDI  using  analytical  redundancy  in  that  it  can  only  monitor  the  systems  or  signals 
that  are  physically  measurable.  An  FDI  algorithm  on  the  other  hand  can  monitor  a 
wider  array  of  parameters.  Even  though  the  sensors  in  a  given  system  are  dissimilar 
they  are  all  driven  by  the  same  dynamic  states  and  are  therefore  functionally  related. 
This  is  what  is  referred  to  as  analytical  redundancy,  inherent  redundancy,  or  func¬ 
tional  redundancy,  as  opposed  to  hardware  redundancy.  The  work  presented  in  this 
thesis  will  be  oriented  towards  the  development  of  an  FDI  algorithm  using  analytical 
redundancy. 

C.  FDI  ALGORITHM  PERFORMANCE  EVALUATION 

Many  criteria  are  used  to  evaluate  the  performance  of  an  FDI  algorithm  but 
the  most  prominent  are: 

•  Rapidity  of  detection. 
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•  Sensitivity  to  slowly  developing  faults. 

•  False  alarm  rate. 

•  Missed  failure  detection. 

•  Incorrect  fault  isolation.  [Ref.  5.  p.  7] 

1.  Failure  Simulation 

The  standard  method  for  testing  the  performance  of  an  FDI  scheme  is  to 
simulate  a  fault,  maintain  it,  and  look  at  the  reaction  of  the  algorithm.  The  faults 
of  a  dynamic  system  can  be  simulated  in  various  ways.  For  example,  a  sensor  fault 
on  a  recorded  signal  can  be  reproduced  by  adding  noise  or  multiplying  the  signal 
by  a  constant,  and  the  behavior  of  the  fault  detection  device  can  then  be  observed. 
This  method  can  be  repeated  to  test  and  improve  the  FDI  algorithm.  More  details 
are  presented  in  a  later  chapter  to  show  how  the  failures  were  simulated  for  the  FDI 
algorithm  presented  in  this  thesis.  It  should  be  mentioned  that  the  technique  of 
emulating  faults  on  one  signal  creates  little  or  no  effect  on  the  other  signals.  This 
situation  could  differ  significantly  from  what  could  happen  in  a  feedback  process 
subjected  to  some  failure.  In  this  case  one  fault  would  often  affect  several  other 
signals. 

2.  Rapidity  of  Detection 

The  output  of  the  detection  system  should  either  indicate  that  a  particular 
system  has  failed  or  give  no  response  if  the  system  is  still  serviceable.  The  rapidity 
of  detection  plays  a  role  depending  on  the  purpose  of  the  FDI  implementation.  In 
aircraft  applications,  the  rapidity  of  detection  is  instrumental  in  ensuring  the  success 
of  the  mission,  especially  on  the  vital  control  sensor  of  jet  aircraft  equipped  with 
stability  augmentation  systems.  However,  if  the  FDI  algorithm  is  to  be  employed 
in  a  system  for  preventive  maintenance  purpose,  the  algorithm  should  be  tuned  to 
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detect  slowly  developing  faults  in  the  form  of  biases  or  noise  at  the  expense  of  speed 
of  detection. 

3.  False  Alarm 

False  alarm  occurs  when  an  error  is  detected  while  the  system  is  still  oper¬ 
ating  properly.  False  alarms  are  indicative  of  a  poor  FDI  algorithm.  Effort  should  be 
made  during  the  design  of  the  algorithm  to  minimize  the  occurence  of  false  alarms  as 
they  lead  to  lack  of  confidence  in  the  system.  Unfortunately  the  minimization  of  the 
false  alarm  rate  is  often  done  at  the  expense  of  detecting  small  errors.  A  trade-off 
between  these  two  criteria  must  be  made.  To  address  this  issue,  the  question  of  how 
important  an  undetected  failure  would  affect  the  system  must  be  answered.  Some¬ 
times  the  seriousness  of  the  fault  is  such  that  it  is  better  to  respond  to  a  false  alarm  by 
changing  the  suspected  component  and  retesting  it  later  than  leaving  it  in  operation. 
This  discussion  leads  to  the  choice  of  thresholds  in  the  algorithm.  Clark  and  Walker 
present  two  interesting  approaches  to  solve  the  threshold  determination  problem  by 
presenting  adaptive  threshold  selection  methods  in  chapters  2  and  14  of  [Ref.  5]. 

4.  Incorrect  Fault  Isolation 

False  identification  of  the  failure  source  is  another  effect  to  be  minimized  in 
the  design  of  the  algorithm.  An  erroneous  identification  of  a  correctly  detected  fault 
causes  the  reconfiguration  system  to  compensate  for  the  wrong  sensor  or  actuator 
and  can  lead  to  multiple  other  failures. 

D.  ROBUSTNESS  OF  FDI  ALGORITHM 

The  performance  criterion  of  the  FDI  algorithm  leads  to  the  concept  of  robust¬ 
ness.  Robustness  of  the  algorithm  is  a  measure  of  how  the  performance  can  remain 
unaffected  by  variation  in  parameters  unaccounted  for  in  the  design  or  by  conditions 
in  the  operating  system  that  vary  differently  than  what  was  assumed  during  the 
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modeling. 

The  primary  sources  of  divergence  from  modeled  deterministic  systems  are  noise 
and  uncertainties  in  the  physical  parameters  of  the  operating  plant.  Most  of  the  signal 
processing  techniques  used  in  the  field  of  FDI  treat  the  problem  of  noise  by  assuming 
that  contributions  have  random  fluctuations  that  follow  stationary  Gaussian  process. 
If  the  actual  system  has  disturbances  and  noise  that  are  non-stationarv  andor  non- 
Gaussian  than  the  FDI  algorithm  performance  will  be  degraded.  Nonlinear  models 
can  also  contribute  to  the  inaccuracy  of  the  model.  In  particular,  when  the  parameters 
of  the  model  are  derived  from  a  linearization  process  around  some  nominal  conditions. 
The  majority  of  the  FDI  techniques  are  based  on  state  estimation  methods  common 
to  linear  systems  theory.  If  the  parameters  of  the  model  are  known  with  precision, 
the  state  estimates  representing  the  modeled  plant  will  be  accurate,  the  FDI  scheme 
will  be  sensitive  to  incipient  faults  and  the  false  alarm  rate  will  be  kept  to  a  minimum 
[Ref.  5,  p.  10]. 

Robustness  with  respect  to  the  type  of  fault  should  also  be  considered  in  the 
design,  since  any  given  components  can  malfunction  in  many  different  ways.  Sensors 
can  experience  biases,  change  of  scale  factor,  wear  and  tear,  friction,  variation  with 
hot  or  cold  temperature,  etc.  All  these  symptoms  can  cause  any  given  component  to 
fail  in  a  different  pattern.  If  the  FDI  algorithm  is  set  to  detect  and  identify  only  a  few 
of  the  many  types  of  faults  for  any  sensor  this  will  lir  ;t  its  performance.  Therefore, 
the  FDI  scheme  should  be  built  on  taking  the  widest  repertoire  of  fault  signatures  and 
should  also  be  capable  of  accommodating  any  new  types  of  failures  as  they  develop. 

E.  PROGRESS  IN  FDI 

Perhaps  one  of  the  most  cited  reference  in  FDI  is  the  paper  published  by  Will- 
sky  in  1976  [Ref.  6].  In  his  paper  Willsky  provides  an  overview  of  a  number  of  the 


7 


basic  concepts  in  failure  detection.  In  particular,  he  concentrates  on  linear  systems 
and  compares  the  design  of  voting  schemes,  specific  failure-sensitive  filters,  multiple 
hypothesis  filter  detectors,  statistical  tests  on  filter  innovations  and  development  of 
jump  process  formulations.  Since  1976,  several  other  survey  papers  have  been  pub¬ 
lished  in  the  field  of  FDI  [Ref.  7,  8,  9j.  New  approaches  have  been  presented  using 
geometrical  interpretation  of  the  concept  of  analytical  redundancy.  These  approaches 
were  developed  for  determining  robust  parity  relations  for  failure  detection  in  dynamic 
systems.  The  differences  in  the  recently  introduced  methods  are  that  the  problems 
of  model  uncertainties  are  now  addressed  in  detail.  [Ref.  10,  2].  To  respond  to  the 
same  problem  of  model  uncertainties,  many  authors  have  worked  with  and  developed 
the  theory  of  Unknown  Input  Observers(UIO)  for  use  in  linear  uncertain  dynamical 
systems  [Ref,  11,  5].  The  last  two  methods  mentioned  are  part  of  what  is  referred 
to  as  the  model-based  approach  to  FDI.  Some  progress  has  also  been  reported  using 
neural  networks  or  the  knowledge  based  approach.  Recent  research  directions  include 
use  of  techniques  such  as  fuzzy  logic,  adaptive  threshold  selector  and  7{0 0  observers 
[Ref.  12]. 

F.  FDI  APPLICATION 

The  FDI  algorithm  developed  in  this  thesis  uses  the  six  degree  of  freedom  model 
derived  for  the  Bluebird  test  aircraft  by  Capt  Kuechenmeister  USMC  [Ref.  13].  The 
non-linear  model  can  be  linearized  around  typical  flight  conditions.  The  Bluebird 
aircraft  is  used  at  the  Naval  Postgraduate  School  to  test  guidance,  navigation  and 
control  systems  in  horizontal  flight.  The  physical  characteristics  of  the  Bluebird  are 
given  in  table  2.3  in  [Ref.  13,  p.  8].  In  an  aircraft  flight  control  system  the  actuators 
are  used  as  servomechanisms  which  drive  the  control  surfaces  and  the  engine.  The 
actuators  receive  their  input  signals  from  an  onboard  Flight  Management  Computer. 
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The  instrumentation  of  the  Bluebird  aircraft  includes  many  sensors  or  transducers 
attached  to  the  airframe,  which  provide  signals  proportional  to  the  motion  of  the  air¬ 
plane.  such  as  airspeed,  altitude,  heading,  accelerations,  attitude  and  rates  of  change 
of  attitude,  control  surface  deflections,  engine  thrust  etc. 

1.  FDI  on  the  Bluebird  Aircraft 

The  four  different  actuators  under  test  in  the  Bluebird  model  correspond 

to  the  following  control  surfaces  and  engine: 

•  Elevator. 

•  Aileron. 

•  Rudder. 

•  Thrust. 

The  sensors  include  the  following  parameters  in  the  Bluebird  model: 

•  u  for  the  forward  velocity. 

•  v  for  the  lateral  velocity. 

•  w  for  the  vertical  velocity. 

•  p  for  the  roll  rate. 

•  q  for  the  pitch  rate. 

•  r  for  the  yaw  rate. 

•  $  (phi)  for  the  Euler  roll  angle. 

•  0  (theta)  for  the  Euler  angle  of  attack. 

•  '5  (psi)  for  the  Euler  heading  angle. 

The  FDI  algorithm  implemented  uses  the  Bluebird  aircraft  model  and  the 
multiple  model  FDI  technique  described  in  [Ref.  7]  and  [Ref.  14].  The  equations  and 
explanation  on  how  this  algorithm  works  are  presented  in  Chapter  II. 

The  work  presented  in  this  thesis  is  related  to  the  Fault  Detection  or  Fault 
Isolation  task  (FDI).  Reconfiguration  or  the  remaining  task  of  the  monitoring  system 
to  reconfigure  and  permit  continued  operation  by  the  observed  plant  after  a  fault  has 
been  detected  and  isolated  is  not  addressed. 
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II.  IMPLEMENTATION  OF  MULTIPLE 
MODEL  TECHNIQUE  FOR  FDI 

A.  DESCRIPTION  ON  THE  MULTIPLE  MODEL  TECHNIQUE 

The  multiple  model  method  addresses  the  problem  of  FDI  by  observing  a  se¬ 
quence  of  inputs  and  outputs  from  a  system  and  then  choosing  one  out  of  a  given  set 
of  possible  models  that  is  felt  most  likely  to  have  responded  in  an  observed  fashion. 
This  broad  definition  for  multiple  model  technique  does  not  only  apply  to  FDI  prob¬ 
lem  but  also  to  areas  of  system  identification  and  adaptive  control  which  prompted 
the  early  development  of  this  method  [Ref.  15]. 

The  approach  to  FDI  using  multiple  model  technique  is  achieved  as  follows: 
the  fault  isolation  is  carried  out  on  the  basis  of  different  fault  hypothesis  and  a 
separate  estimator  (observer  or  Kalman  filter)  is  assigned  for  a  finite  number  of  fault 
hypotheses.  These  hypotheses  are  then  tested  in  terms  of  likelihood  functions,  using 
Bayesian  decision  theory,  to  detect  which  fault  is  present.  The  algorithm  presented 
in  this  thesis  makes  use  of  a  bank  of  14  Kalman  filters  which  represent  each  of  the 
different  error  hypotheses  tested.  The  first  hypothesis  is  in  the  case  of  no  failure.  The 
remaining  filters  are  tuned  for  hard  failure  in  the  four  actuators  and  nine  sensors. 
Figure  2.1  represent  the  flow  diagram  for  the  multiple  model  FDI  algorithm. 

B.  STATE  EQUATIONS  OF  THE  AIRCRAFT 

The  states  of  the  observed  plant  can  be  represented  by  a  set  of  linearized  stochas¬ 
tic  equations  in  the  time  domain.  In  the  following  equation  the  input  is  represented 
by  u,  the  output  vector  by  y  and  the  states  of  the  system  by  the  vector  x.  All  other 
effects  that  obscure  the  fault  detection  are  introduced  in  the  vectors  w  and  v  and 
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Figure  2.1:  FDI  Flow  Diagram  using  Multiple  Model  Algorithm 

include  parameters  such  as  noise  or  unknown  inputs: 

x(t)  =  Ax(t)  +B  u{t)  +W®(i)  (2.1) 

y{t)  =  Cx(t)  +Vv(t)  (2.2) 

In  the  Bluebird  application,  x  G  ft9,  u  €  ft4  and  y  €  ft9.  The  matrices  A,  B,  C  are 
known  and  of  compatible  dimensions. 

1.  State-Space  Representation  of  Sampled  and  Discrete  Systems 

The  solution  to  equation  2.1  has  the  following  form: 

x(t)  =  $(t,t0)x(t0)  +  /  $(t,r)B  u(r)  dr  +  /  $(l,r)Wffi(r)(ir  (2.3) 

•'to  J  to 
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If  the  system  is  sampled  with  sampling  interval  T.  then  we  are  interested 
in  the  values  of  the  state  x  at  discrete  time  intervals.  Using  equation  2.3.  the  value 
of  the  state’s  vector  x,  at  time  t  =  (k+  l)T  given  its  value  at  t0  =  kT  can  be  obtained 
as  follows: 

f(k+l)T 

x({k+\)T)  =  *[{k+l)T.kT]x{kT)  +  J  #((fc  +  l)T.r)B  u(t)  dr  + 

r(k+l)T 

/  ${{k  +  1)T,  t)W  w(t)(It  (2.4) 

JkT 

If  u(-)  and  w(-)  are  piecewise  constant  between  sampling  intervals,  that  is. 

u(t)  =  u(kT)  for  kT  <  t  <  (k  +  1  )T 

and 

w(t)  =  w(kT)  for  kT  <  t  <  (k  +  l)T 

then  u  and  w  can  be  taken  out  of  the  integral  in  equation  2.4  to  obtain  the  sampled 
or  discrete  state-space  representation 

x((*+ 1  )T)  =  *[(*+  1)T,  kT]  x(kT)  +  fA[(k+l)T,  kT]  u(kT)  +  r[(Jb+ 1  )T,  kT]  w(kT ), 

(2.5) 

where 

r(k+l)T 

&[(k  +  1)T,  kT]  =  /  exp{A[(k  +  1)T,t]}  dr  (2.6) 

JkT 

is  the  state  transition  matrix  in  the  discrete  form  and 


A[(k+l)T,kT]  = 

f(k+l)T 

/  *[{k+  l)T.r]Bdr 

JkT 

(2.7) 

r[(jfc  +  i)r,Jkr]  = 

r(k+i)T 

/  *[(*  +  l)7\r]Wdr 

JkT 

(2.8) 

A  discrete  measurement  y  is  given  by 

y(jfcT)  =  C{kT)x(kT)  +  V(kT)v{kT)  (2.9) 
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If  the  system  is  not  a  sampled  continuous  system,  then  T  can  be  ignored  and  set  to 
one  [Ref.  16,  p.  215].  The  solution  to  the  difference  equation  2.5  can  be  obtained  by 

letting  k  take  on  values  k  =  0, 1,2,3 . .V  and  collecting  terms  to  get  the  solution 

of  the  discrete-time  equation  over  kT  samples: 

k— 1  k-l 

x(kT)  =  *{T)  x(0)  +  Y.  *k~J~l(T)  A(T)  u(jT )  +  £  ^~l(  T)  T(T)  w(jT)  (2.10) 

j=0  j=0 

The  format  used  in  the  rest  of  the  document  as  well  as  in  the  code  of 
the  FDI  algorithm  will  utilize  the  nomenclature  employed  in  [Ref.  17]  which  is  a 
simplification  of  the  equations  derived  above.  The  following  equations  represent  the 
state  and  measurement  equations  of  a  linear  plant  using  this  simplified  format.  Notice 
that  the  vectors  are  represented  bv  using  bold  characters.  In  the  Bluebird  application, 
the  C  matrix  will  be  set  to  be  identity  since  all  measurements  are  equal  to  the  state 
of  the  plant: 

x(k  +  l)  =  <P(k  +  1,  k)x(k)  +  A(k  +  1,  k)  u(k)  +  T(k  +  1,  k)  w(k)  (2.11) 
y(Jfc)  =C  (k)x(k)  +  V(k)v{k)  (2.12) 

C.  KALMAN  FILTER  EQUATIONS 

The  following  reasons  prompted  the  use  of  Kalman  filters  to  detect  failures. 
First  the  algorithm  of  the  Kalman  filter  allows  the  calculation  of  the  gain  matrix  in 
order  to  minimize  the  variance  of  the  estimation  error.  This  feature  of  optimization 
will  be  beneficial  if  the  same  estimates  obtained  from  the  FDI  algorithm  are  to  be  used 
later  on  in  the  aircraft  autopilot.  Another  advantage  of  using  an  estimation  algorithm 
is  that  in  the  absence  of  hardware  redundancy  it  is  still  possible  to  use  a  degraded 
instrument  through  the  information  provided  by  the  estimator  in  the  reconfiguration 
mode.  However,  the  main  reason  for  using  Kalman  filters  is  that  the  covariance  of 
the  residual  or  innovation  process  calculated  in  order  to  determine  the  Kalman  gains 
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are  also  utilized  in  the  detection  calculation  using  Bayesian  theory.  This  feature  of 
the  detection  algorithm  will  be  covered  in  more  detail  in  section  D. 

The  equations  developed  in  the  following  sections  are  implemented  in  the  MaTLAB 
code  listed  in  APPENDIX  C. 

1.  Assumptions  and  Initial  Values 
a.  Noise  Assumptions 

With  the  plant  equations  given  by  2.11  and  2.12.  the  following  as¬ 
sumptions  are  made:  The  measurement  noise  has  zero  mean. 


E[v(k)]  =  0.  for  k  =  0. 1.2.... 


(2.13) 


is  uncorrelated. 


E[v{k)vT{])}  =  E[v{j)vT(k)}  =  0.  for  j  ^  k 


(2.14) 


and  has  covariance 


E[v{k)vT(k)]  =  R (fc),  for  k  =  0,1,2,... 


(2.15) 


Equations  2.14  and  2.15  can  also  be  expressed  with  the  Kroneker  delta  as 


E[v(k)vT(j)]  =  Rf  k)6kj 


(2.16) 


where 


c  ■  0,  k  /  j 

Sil=  i,  t.j 

The  random  process  noise  has  zero  mean 


(2.17) 


E  (w(&)]  =  0,  for  k  -  0, 1, 2, . . . 


(2.18) 


is  uncorrelated,  and  has  covariance 


E[w(k)wT(j )]  =  E[w{j)wT{k)}  =  Q  (k)Skj 


(2.19) 
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The  random  process  noise  and  the  measurement  noise  are  uncorrelated 

£[w(Ar)vT0')]  =  E  [v(ji)wT(A:)]  =  0,  for  k.j  =  0. 1,2 -  (2.20) 

b.  Initial  States  Assumptions 

The  input  u (k)  for  k  =  0. 1,2, . . .  is  known  and  deterministic. 


The  initial  state  is  a  random  variable  with  known  mean 

E[x(0)j  =  (2.21) 

and  covariance 

E  {  [x(0)  -  x^][x(0)  -  x^j7  }  =  M  (2.22) 

the  measurement  noise  and  initial  states  are  uncorrelated: 

E  [x(0)  vT(/;)]  =  E  [v(/:)  xt(0)]  =  0.  for  k  =  0,1,2 _  (2.23) 

The  random  process  noise  and  initial  state  values  are  uncorrelated  as  well: 

E  [w(fc)xT(0)|  =  E  [x(0)wT(fc)]  =  0,  for  k  =  0,1,2,...  (2.24) 

2.  Discrete  Kalman  Filter  Equations 

The  observer  equation  is  given  by 

x(k/k)  =  x(k/k  -  1)  +  G(k)  [y(k)  -  y(fc)]  (2.25) 

where 

y  (k)  =  Cx(k/k-l)  (2.26) 


The  nomenclature  x(k/k  —  1)  means  estimate  of  x  at  time  or  observation  k  given 
measurements  at  time  up  to  and  including  (k  -  1).  The  goal  of  the  optimal  estimator 
is  to  minimize  the  estimation  error  defined  as 

e(k/k)  =f  x(k/k)  -  x(k)  (2.27) 
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A  zero  mean  estimation  error  is  sought  for  all  k  to  obtain  an  unbiased  estimator. 
The  complete  development  of  the  gain,  covariance,  and  update  matrices  in  order  to 
achieve  the  minimization  of  the  variance  of  the  measurement  errors  can  be  found  in 
[Ref.  17,  p.4-32  to  4-41].  The  following  equations  summarize  the  different  parameters 
involved  in  the  calculations  inside  the  Kalman  filter.  The  computational  steps  involve 
calculations  before  use  of  the  Kalman  filter  as  well  as  generation  of  estimates  during 
the  filter  operation.  The  gain  schedule  can  be  evaluated  prior  to  using  the  estimator 
since  the  gains  do  not  depend  on  the  measurement  data  sequence  [Ref.  17.  p4— 42] . 

The  following  calculations  are  used  to  compute  the  gains  and  are  imple¬ 
mented  in  the  MATLAB  file  kalmn\_gain.m  found  in  APPENDIX  C. 

First,  let  k  =  0  and  set  the  initial  value  for  the  covariance  matrix  P(k/k  — 
1 )  —  M  and  then  calculate  the  gain  schedule  with 

G(Jfc)  =  P(k/k  -  1)  C T{k)  [C(Jfc)  P(J fc/ifc  -  1)  C T(k)  +  R (k)  l"1  (2.28) 

The  term  in  the  bracket  of  equation  2.28  represents  the  hypothesized  filter’s  internally 
computed  residual  covariance  or  the  innovation  covariance.  This  term  will  become 
important  in  section  D.  and  is  denoted  as 

a-, (k)  =  C(k)P{k/k  -  l)CT{k)  +  R (k)  (2.29) 

Next,  compute  the  covariance  of  the  estimation  error  matrix 

P(k/k)  =  [I  -  G(k)  C(fc)]  P(k/k  -  1)  (2.30) 

Now,  compute  the  predicted  value  for  P(k  +  1  /k)  using  the  discrete  Riccati  equation: 

P(k+l/k)  =  $(k  +  l,k)P(k/k)$T{k  +  l,k)  +  T(k  +  l.k)Q{k)T(k+l.k)  (2.31) 

Repeat  equation  2.28,  2.30  and  2.31  recursively  until  the  final  observation  value 
for  k  is  reached.  Hopefully,  k  should  be  large  enough  so  that  the  gains  will  have 
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reached  steady-state  values.  Notice  that  the  last  value  calculated  for  P{k  +  l/k) 
becomes  P(k/k  —  1)  when  the  values  are  brought  up  again  at  the  beginning  of  this 
computation  loop. 

After  the  gain  schedule  has  been  computed,  the  values  are  stored  in  memory 
and  the  observer  can  be  implemented.  The  next  equations  are  required  to  build  the 
Kalman  filter.  Once  again  these  equations  can  be  evaluated  on  a  digital  computer. 
The  MATLAB  code  for  the  Bluebird  application  can  be  found  at  APPENDIX  C  in 
file  kalman_i.m.  First,  k  is  set  to  zero  and  x(0/  —  1)  is  initialized  to  x^.  After  this, 
x(k/k)  can  be  evaluated  when  the  first  observation  y (k)  becomes  available  with 

x(k/k)  =  x(k/ k  —  1 )  +  G{k)[y(k)  -  C(k)  x(k/k  -  1)]  (2.32) 

The  term  in  the  bracket  of  equation  2.32  is  referred  to  as  the  innovation  process  or 
the  residual  and  is  represented  by 

rt(k)  =  y (k)  -  y(k/k  -  1)  (2.33) 

The  predicted  value  for  x(k  +  1/A:)  is  then  evaluated  with 

x(A:-f  1/A;)  =  $(A;  +  1,  A;)  x(k/k)  +  A(k  +  i,k)  u(k)  (2.34) 

Equations  2.32  and  2.34  are  also  repeated  recursively  to  generate  the  state  estimates. 
As  in  the  previous  procedure,  the  last  value  calculated  for  x(A;  +  l/A’)  becomes  x(k/k  — 
1)  when  the  loop  is  reentered. 

The  updated  measurement  estimates  are  also  calculated  in  the  above  steps 
in  equation  2.32  since 

y(k)  =  C(k)x{k/k-l)  (2.35) 

D.  PROBABILITY  OF  FAILURE  USING  BAYESIAN  THEORY 

As  explained  earlier,  the  Multiple  Model  algorithm  makes  use  of  Bayesian  theory 
to  perform  the  task  of  detection  and  isolation.  The  Multiple  Model  technique  involves 
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the  design  of  a  finite  set  of  linear  stochastic  systems  indexed  by  t  =  1,2 . V  with 

the  it/l  model  being  the  one  that  corresponds  to  the  actual  model  being  observed. 
This  leads  to  the  standard  multiple  hypothesis  testing  problem.  Suppose  that  H, 
denotes  the  hypothesis  that  the  real  system  corresponds  to  the  tth  model,  and  p,(0) 
the  a-priori  probability  that  Hi  is  true,  then  similarly  pi(k)  can  denote  the  probability 
that  Hi  is  true  for  all  the  measurements  up  to  and  including  the  kth  observation.  The 
previous  measurements  can  be  represented  by  the  set  Mk  =  {u(0),u(l),...,u(Ar  — 
l),y(l),y(2), . . . ,  y{k  —  1)}.  The  Bayes'  rule  gives  the  following  formula  to  calculate 


Pi{k), 


p(  y(k)\Hj,  Mk  )  pj{k  -  1) 

Zl,p(y(k)\HJ,Mk)pJ(k-l) 


(2.36) 


where  p(-)  is  the  conditional  probability  density  function.  The  density  functions 
must  be  calculated  at  each  observation.  This  density  function  is  conditioned  on  Hi 
and  is  fortunately  the  same  one  step  prediction  density  function  produced  by  the 
ith  Kalman  filter.  This  term  is  represented  by  a,  in  2.29.  Under  hypothesis  //,, 
the  residual  rt(k)  should  have  a  mean  of  zero,  a  covariance  of  at(k)  and  be  normally 
distributed.  Moreover,  y(k)  conditioned  on  Hi  and  Mk  should  be  Gaussian  with  mean 
Ci(k)xi(k/k  —  1)  and  with  covariance  a,(fc).  [Ref.  7,  p.  2-2]  These  assumptions  lead 
to  the  following  important  formula  for  the  probability  density  function 


p(y(k)\Hi,Mk)  = 


1 


7-exp{— irf  (k)  a,  1(A;)  rt(A:)}  (2.37) 


(27r) T[det  a,(A:)]i 
where  m  is  the  dimension  of  y(k). 

Equations  2.32,  2.33,  2.34,  2.36  and  2.37  represent  the  Multiple  Model 
algorithm. 
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III.  ADJUSTMENTS  TO  THE  MULTIPLE 
MODEL  ALGORITHM 


A.  MODEL  SELECTION  FOR  EACH  FILTER 

As  mentioned  in  Chapter  II.  .  section  A.,  the  Multiple  Model  algorithm  is 
implemented  using  a  bank  of  Kalman  filters.  Each  Kalman  filter  is  based  or  "tuned" 
for  one  possible  error  scenario.  The  inputs  to  the  filters  are  the  measurements  of  the 
different  aircraft  sensors  and  the  outputs  are  the  innovation  sequences  rt{k ),  which  are 
a  measure  of  how  close  the  estimates  are  to  the  true  measurements.  These  residuals 
should  be  white  sequences  for  the  filter  tracking  the  correct  model.  If  the  filters  model 
is  not  correct,  then  the  residuals  will  not  be  white  and  will  include  errors  due  to  the 
fact  that  the  estimates  are  based  on  incorrect  models.  The  probability  calculations 
equation  2.36  give  a  measure  of  which  model,  based  on  a  specific  fault,  is  most  likely 
to  be  correct  compared  to  other  models. 

1.  Fault  Modeling  for  the  Filters  and  for  the  Simulation  Runs 

Each  Kalman  filter  is  based  on  a  particular  failure;  several  methods  can  be 
used  to  change  the  nominal  model,  i.e.,  the  model  representing  no  failure,  to  one  that 
is  modified  to  isolate  a  specific  fault. 
a.  Actuator  Failure 

Typically,  actuator  malfunction  can  be  modeled  by  setting  one  of  the 
columns  of  the  B  matrix  to  zero  for  the  respective  actuator  to  be  at  fault.  Another 
way  is  to  increase  the  covariance  of  w(k )  over  the  normal  range  of  operation.  A 
third  way,  equivalent  to  the  previous  two,  would  be  to  add  a  driving  term  g(k)  in 
equation  2.34,  [Ref.  8,  p.461].  The  algorithm  developed  in  this  thesis  will  utilize 
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the  first  two  schemes  to  simulate  faults  and  only  the  zeroing  scheme  to  set  the  model 
used  by  each  Kalman  filter.  This  is  done  in  order  to  keep  the  number  of  Kalman 
filter  to  a  minimum.  Work  by  Menke  and  Maybeck  has  also  indicated  that  soft 
failure  can  be  detected  by  Kalman  filters  tuned  with  hard  failure  models  [Ref.  14, 
p.  3136].  Therefore,  the  method  employed  in  this  thesis  will  use  the  first  modeling 
scenario  where  the  columns  of  the  B  matrix  are  set  to  zero  to  tune  the  Kalman  filters. 
However,  during  the  simulation,  the  appropriate  column  of  the  system's  B  matrix  fed 
into  all  the  different  Kalman  filters  will  only  be  set  to  zero  for  simulation  of  hard 
actuator  failure.  Incipient  actuator  failure  simulation  will  be  done  by  changing  the 
covariance  of  the  process  noise  for  the  system’s  model.  In  other  words,  Kalman  filters 
based  on  the  models  with  B  matrice’s  column  set  to  zero  will  be  expected  to  detect 
both  hard  and  soft  failure  for  the  one  specific  actuator. 
b.  Sensor  Failure 

Sensor  failures  can  also  be  modeled  in  three  ways.  The  methods  are 
the  same  as  for  the  three  schemes  explained  above  for  the  actuators  except  that 
the  zeros  are  included  in  the  respective  sensor’s  column  of  the  C  matrices  and  the 
measurement  noise  v(k )  is  varied  instead  of  the  process  noise. 

B.  TUNING  OF  THE  KALMAN  FILTERS 

After  selecting  the  models  for  the  design  of  each  filter,  it  is  important  to  set  the 
parameters  of  every  Kalman  filter  so  that  the  process  of  detection  will  be  optimized 
to  prevent  the  occurrence  of  false  alarms.  The  Kalman  filter  is  said  to  be  tuned  if  it 
provides  an  optimal  or  minimal  error  covariance  on  the  estimates  of  the  states. 

The  performance  of  the  filter  can  be  evaluated  and  adjusted  by  a  series  of 
statistical  tests.  The  operation  of  the  estimator  in  equation  2.32  can  be  simplified 


20 


by  the  following  representation 


xneti;  —  x old  T  GI  [residualsnetl,]  (3.1) 

From  equation  3.1,  it  is  easy  to  see  that  the  new  estimates  are  formed  partly  using 
the  old  estimates,  which  are  a  function  of  the  plant  model,  and  also  from  the  new 
measurements  residuals.  As  one  can  see,  the  value  of  the  new  estimates  is  also  a 
function  of  the  Kalman  gains.  For  small  G’s,  the  new  estimates  use  the  model  and 
for  large  gains,  the  new  estimates  will  be  influenced  by  the  measurements.  The  choice 
and  derivation  of  the  gains  is  therefore  instrumental  in  the  operation  of  the  Kalman 
filters. 

1.  Influence  of  the  Kalman  Filter’s  Gains 

As  can  be  seen  in  equation  2.28,  the  gains  are  influenced  by  two  variables, 
P(k/k  —  1)  and  R(k).  Thus,  there  are  four  ways  to  influence  the  estimates.  First 
P(k/k  —  1)  are  small  and  R(k)  are  fixed  which  gives  small  gains  and  is  in  agreement 
with  the  model  behavior  since  small  P(k/k  —  1)  means  that  the  model  is  adequate 
(small  variances  on  the  errors).  Second,  R(k)  are  large  and  P(k/k  —  1)  are  fixed, 
which  leads  to  small  gains  and  also  means  that  the  model  should  be  believed  since 
the  measurements  are  noisy.  Now  for  the  case  where  the  gains  are  large.  If  the 
P(k/k  —  1)  is  large  and  R(k)  is  fixed  then  the  model  is  inadequate  and  the  new 
measurements  should  be  used.  If  R(k)  is  small  and  P{k/k  —  1)  is  fixed,  then  it  means 
that  the  measurements  are  good. 

A  Kalman  filter  does  not  operate  correctly  if  the  gains  becomes  small  before 
they  should,  meaning  that  the  filter  thinks  that  the  new  measurements  do  not  carry 
valuable  information  when  this  is  actually  not  the  case.  The  filter  is  said  to  diverge 
when  this  occurs.  To  avoid  filter  divergence  a  series  of  tests  must  be  performed. 
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2.  Tests  on  the  Kalman  Filter 

As  a  first  check  for  the  operation  of  the  Kalman  filter,  the  innovations 
should  form  zero-mean  and  white  sequences  (see  proof  in  [Ref.  IS]).  Formulae  can  be 
found  in  [Ref.  16,  p.95-97]  to  evaluate  the  mean  with  test  statistics  and  to  perform 
whiteness  tests  on  the  covariance  of  the  residuals.  These  tests  can  also  be  performed 
in  MATLAB  using  the  mean(-)  and  cov(-)  functions. 

3.  The  Filter  Knobs 

The  different  parameters  available  in  the  Kalman  filter  for  tuning  will  be 
discussed  in  this  section.  The  normal  approach  taken  for  state  estimation  in  aircraft 
application  is  to  use  a  model  developed  from  the  aircraft's  equations  of  motion,  test 
i;  to  estimate  the  noise  statistics  then  construct  the  filters  and  adjust  them  using 
the  data  collected  in  the  testing.  Once  these  steps  have  been  done  the  filters  can  be 
evaluated  to  test  their  performance.  If  the  designer  is  not  satisfied  with  the  results, 
adjustments  are  done  until  satisfactory  performance  is  obtained.  This  constitutes  the 
phase  called  “tuning  of  the  Kalman  filter” . 

Various  parameters  are  changed  during  the  tuning  process.  These  param¬ 
eters,  sometimes  called  “filter  knobs”,  are  the  process  noise  covariance  Q{k),  the 
measurement  noise  covariance  R(k),  and  the  initial  condition  on  F(0/0).  From  equa¬ 
tion  2.31,  it  is  apparent  that  for  large  Q(k),  P(k  +  l/k)  will  also  become  large 
indicating  high  uncertainties  and  an  unreliable  model.  Conversely,  if  the  values  for 
Q(k)  are  small,  P(k  +  1/A:)  will  be  small  and  the  model  will  become  adequate. 

Another  way  to  look  at  varying  the  knobs  is  to  observe  their  effects  on  the 
filter’s  transient  response  and  the  noise  in  the  state  estimates.  As  Q(k)  increase  so 
will  the  gains  and  the  system  transient  performance  will  be  faster.  However  in  this 
case  the  noise  in  the  state  estimates  will  be  larger.  The  same  effect  would  occur  if 
R(k)  would  be  smaller,  but  in  many  cases,  the  noise  on  the  sensors  is  fixed.  (See 
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Table  4.2).  The  second  possibility  is  to  lower  values  lor  Q(k).  This  results  in  lower 
gains,  and  therefore  in  a  slower  filter  transient  performance.  However,  this  would  also 
filter  more  noise  from  the  estimates. 

The  initial  values  for  P(0/0)  also  have  an  effect  on  the  algorithm.  As 
P( 0/0)  is  set  to  large  values,  the  initial  gains  will  also  become  large  and  the  initial 
measurement  will  be  heavily  weighted  compared  to  the  model  which  will  be  considered 
inadequate.  It  should  be  noted  however  that  as  more  observations  become  available 
the  effects  of  the  initial  values  of  P(0/0)  disappear.  This  effect  is  favorable  as  the 
initial  values  for  P(0/0)  are  seldom  known.  These  results  will  occur  only  if  the  filter 
is  stable  and  the  system  is  observable  and  controllable.  [Ref.  16] 

C.  PROBLEMS  WITH  THE  PROBABILITY  CALCULATIONS 

A  minimum  value  problem  can  arise  when  using  equation  2.36.  If  the  value 
calculated  for  p,(k)  is  small  the  next  value  pt(k  +  1)  will  only  grow  back  slowly  if  it 
has  to.  If  pi(k)  becomes  zero,  then  the  next  values  for  p,(n)  for  n>  k  will  also  be  set 
to  zero. 

To  avoid  the  problem  of  slow  variation  in  the  pi(k)  calculation  and  the  generation 
of  null  values,  the  p,(fc)  are  set  to  a  minimum  value  if  the  calculations  fall  below  a 
certain  level.  The  bound  has  been  set  to  10~4  for  the  application  studied  in  this  thesis. 
This  number  can  be  changed  easily  in  the  MATLAB  file  pk.m  under  the  variable  name 
“min”  for  any  other  value.  However,  this  one  seemed  to  work  fairly  well  for  the 
Bluebird  application  and  lies  in  the  range  recommended  by  Willsky  [Ref.  7,  p.2-5]. 


23 


D.  PERFORMANCE  OF  THE  MULTIPLE  MODEL  ALGORITHM  IN 
DIFFERENT  ENVIRONMENTS 

1.  Non-Gaussian  Noise 

The  Gaussian  noise  assumption  is  made  in  the  formulation  of  equation  2.36 
more  specifically  in  the  density  function  of  equation  2.37.  According  to  Willskv,  [Ref. 
7],  even  in  the  presence  of  non-Gaussian  residuals,  the  performance  of  the  algorithm 
should  not  be  influenced.  That  is,  the  algorithm  is  designed  to  measure  how  well 
each  of  the  Kalman  filters  is  tracking  by  observing  the  residuals  that  are  produced. 
The  probability  calculations  of  equation  2.36  only  measure  now  well  each  filter  is 
estimating  data  relative  to  each  other  and  how  well  they  are  expected  to  be  tracking. 
The  important  term  in  equation  2.37  is 

rj{k)a;l(k)n(k).  (3.2) 

In  fact  previous  efforts  have  noted  the  leading  coefficient  before  the  exponential  in 
equation  2.37  provides  little  information  in  the  identification  of  a  failure  [Ref.  14]. 
If  the  likelihood  quotient  that  equation  3.2  represents  is  large,  the  ith  model  will  be 
non  valid  and  if  it  is  small,  then  the  ith  model  is  tracking  well.  This  is  reflected  in 
the  calculation  for  the  pi{k)  and  the  Multiple  Model  algorithm  will  still  produce  the 
expected  results  in  the  presence  of  non-Gaussian  statistics. 

2.  Non-Linearities 

The  usual  approach  to  non-linearities  is  to  linearize  the  system  around  a 
number  of  operating  points  that  reflect  the  flight  condition  of  the  aircraft  for  each  of 
the  models  in  the  algorithm.  These  linearized  models  span  the  flight  envelope  and 
can  then  be  used  to  design  extended  Kalman  filters  which  can  replace  the  standard 
Kalman  filters  in  the  Multiple  Model  algorithm.  This  has  been  done  in  this  thesis 
but  around  one  operating  point  only.  The  A  and  B  matrices  were  generated  by  Lt  D. 
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TABLE  3.1:  NOMINAL  CONDITIONS  FOR  THE  BLUEBIRD 


Turn  rate 

^21B£SS9li 

Ground  speed 

300  (ft /min) 

Hallberg  USN  in  his  present  thesis  work  to  design  a  controller  for  a  nonlinear  system, 
such  as  the  flight  control  system  of  the  Bluebird  .  The  plant  was  linearized  around 
the  conditions  in  Table  3.1  and  the  numbers  in  the  A  and  B  matrices  were  generated 
using  the  MATLAB  function  ‘‘linmod”.  The  values  are  shown  in  APPENDIX  C  in 
file  ABCUJoad.m.  The  C  matrix  is  set  as  the  identity. 

The  problem  associated  with  the  linearized  approach  is  to  determine  whether 
the  tracking  errors  from  the  extended  Kalman  filters  corresponding  to  the  linearized 
model  are  significantly  smaller  than  the  errors  from  filters  based  on  more  distant 
models.  In  the  nonlinear  case,  the  inaccuracies  of  the  Kalman  filters  increase  the  val¬ 
ues  of  the  internally  generated  residual  covariance  at(k)  in  equation  2.29  and  2.28. 
This  has  the  effect  of  reducing  the  value  of  the  probabilities  pi(k)  obtained  through 
equation  2.37.  As  the  values  for  a{(k)  increase  it  becomes  more  difficult  to  detect 
errors. 
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IV.  SIMULATION  AND  RESULTS  FOR  THE 
MULTIPLE  MODEL  FDI  ALGORITHM 

The  procedure  to  simulate  faults  and  test  the  Multiple  Model  FDI  algorithm  is 
explained  in  APPENDIX  A. 

The  simulation  included  trials  to  simulate  hard  failures  in  the  actuators,  hard 
allures  in  the  sensors,  soft  failures  in  the  actuators  and  soft  failures  in  the  sensors. 
The  results  are  shown  in  APPENDIX  B.  An  input  function  was  introduced  in  the 
system  in  order  to  generate  fluctuations  in  the  sensors  and  drive  the  states  to  nonzero 
values  (see  file  kalman\_i  ,m  in  APPENDIX  C).  If  a  state  or  sensor  is  at  or  close  to 
zero  it  is  very  difficult  or  impossible  to  detect  a  failure  that  is  based  on  the  hypothesis 
that  the  sensor  or  actuator  will  go  to  zero  when  it  fails. 

The  following  paragraphs  will  introduce  some  of  the  data  used  to  initialize  the 
algorithm  prior  to  testing  as  well  some  discussion  of  the  results. 

A.  SIMULATION  PARAMETERS 

As  discussed  in  chapter  III.  section  B.  3.,  the  value  chosen  for  the  process  noise 
will  influence  the  behavior  of  the  filters.  The  numbers  employed  in  the  simulation 
are  shown  in  Table  4.1  and  can  be  changed  in  file  ABCUJoad.m.  These  values 
were  selected  to  be  as  low  as  possible  in  order  to  have  each  of  the  separate  filters 
tuned  to  follow  their  respective  model  closely  and  to  put  little  weight  on  the  new 
measurements.  This  is  done  to  isolate  the  effect  of  a  hard  failures  in  each  specific 
filter. 

It  was  found  that  in  certain  runs  where  the  process  noise  values  were  too  low.  the 
numbers  generated  by  the  Kalman  filters  in  Matlab  would  become  unstable  or  out 
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TABLE  4.1:  PROCESS  NOISE  VALUES 


State 

Value 

u 

1.50000  (ft/sec) 

V 

0.75000  (ft/sec) 

w 

0.75000  (ft/sec) 

P 

0.08550  (rad/sec) 

q 

0.08550  (rad/sec) 

r 

0.60000  (rad/sec) 

$ 

■inn 

0 

0.00785  (rad) 

$ 

0.02618  (rad) 

of  range  and  the  output  of  the  probability  computation  were  assigned  to  NaN  (Not 
a  Number).  NaN  is  produced  in  MATLAB  when  a  division  of  either  zero  by  zero  or 
infinity  by  infinity  occurs.  To  fix  this  problem,  numbers  assigned  for  the  process  noise 
were  increased  until  the  algorithm  could  function  properly.  Andrews  and  Grewal  in 
[Ref.  19,  p.  192—260]  provide  several  interesting  implementation  methods  for  Kalman 
filters  where  ill-conditioned  problems  arise.  None  of  these  special  methods  have  been 
tested  in  this  thesis  but  they  should  be  investigated  if  more  accuracy  is  to  be  required 
in  future  usage  of  this  algorithm. 

The  values  used  for  the  measurement  noise  are  those  provided  by  the  manu¬ 
facturer  of  the  sensor.  These  numbers  are  given  in  Table  4.2  and  were  obtained 
from  [Ref.  13,  p.  60,61,71].  Samples  of  white  noise  were  generated  in  Matlab  to 
be  represented  by  w  and  v  in  equations  2.11  and  2.12  and  are  shown  in  figure  B.l 
and  B.2. 

B.  KALMAN  FILTER  TESTS 

The  Kalman  filter  developed  for  the  algorithm  was  first  tested  using  the  plant 
described  in  [Ref.  16,  p.  127]  and  [Ref.  19,  p.  143]  and  the  error  covariance  matrices 
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TABLE  4.2:  MEASUREMENT  NOISE  VALUES 


State 

Value  | 

u 

1.00000  (ft /sec)  ! 

Q|| 

0.50000  (ft/sec) 

w 

0.50000  (ft/sec) 

p 

0.57000  (rad/sec) 

q 

0.57000  (rad/sec) 

r 

0.57000  (rad/sec) 

$ 

0 

0.00873  (rad) 

matched  those  in  the  referenced  books.  The  covariance  matrices  for  the  simulation 
runs  are  shown  in  Figures  B.3  -  B.ll.  All  converge  to  small  values  as  expected. 

The  gains  for  the  bank  of  Kalman  filters  are  shown  in  Figures  B.12  -  B.20. 
The  values  for  the  gains  converges  to  zero  or  close  to  it.  This  was  expected  since  the 
process  noise  numbers  were  chosen  to  be  small  in  order  to  get  small  gains.  This  way 
the  filters  would  trust  the  model  derived  for  their  appropriate  failure  hypothesis. 

The  measurement  state  est'mates  or  filtered  measurements  were  plotted  against 
the  noisy  measurements  in  Figures  B.21  -  B.29  for  the  case  of  no  failure.  On  all  the 
figures,  the  estimates  from  the  Kalman  filters  follow  the  general  curve  or  orientation 
of  the  “true”  measurements  which  was  expected  from  the  theory.  The  “notch”  seen 
on  some  of  the  figures  at  the  100t/l  observation  is  caused  by  the  step  input  function 
that  comes  down  at  this  point.  The  residuals  or  innovation  processes  are  also  shown 
in  Figures  B.30  -  B.38  and  are  used  in  the  calculation  of  the  probability  of  detecting 
error  in  equation  2.36. 


28 


C.  DETECTION  AND  ISOLATION  RESULTS 


Menke  and  Maybeck  in  [Ref.  14,  p.  3136]  decided  to  set  the  detection  threshold 
for  the  different  failure  scenarios  to  a  repetitive  probability  of  0.9  or  higher  for  ten 
consecutive  observations  in  their  application  of  FDI.  This  definition  sets  a  minimum 
that  was  well  exceeded  when  testing  this  algorithm  for  the  hard  failure  scheme.  A  set 
of  13  hard  failures  were  tested  and  some  of  the  results  are  plotted  in  Figures  B.39 
-  B.51.  All  of  the  data  was  not  shown  due  to  the  large  volume  generated.  Every 
run  for  each  failure  generates  13  graphs  of  probability  computation  and  only  one  of 
those  should  show  values  close  to  one.  The  algorithm  was  quite  good  at  detecting 
abrupt  failures  in  the  system  since  all  the  probability  plots  show  values  reaching  one 
fairly  soon  and  staying  at  that  level  until  the  end  of  all  the  observations.  The  plots 
shown  in  Figures  B.39  -  B.51  represent  only  the  particular  output  from  the  filter  of 
interest  for  the  particular  failure  that  they  were  supposed  to  be  tuned  to  detect.  The 
otner  plots  from  the  filters  tuned  for  other  failures  gave  values  of  probability  in  the 
vicinity  of  zero. 

The  test  for  no  failure  was  also  successful  and  the  filter  tuned  for  no  failure 
resulted  in  probability  computation  of  one  fairly  early  in  the  simulation  as  seen  in 
Figure  B.52. 

The  results  obtained  during  the  soft  or  incipient  failure  runs  were  incoclusive 
and  the  algorithm  used  in  this  detection  scheme  should  not  be  considered  yet  as 
operating  satisfactorily.  The  probability  computations  were  not  consistent  from  one 
run  to  another  and  several  false  alarms  were  generated,  i.e.,  probabilities  of  0.9  were 
reached  on  filter  channels  not  tuned  to  detect  the  error  being  tested.  The  plots  in 
Figures  B.53  -  B.55  were  generated  for  the  soft  failure  simulation  on  the  elevator, 
Figure  B.53  shows  that  the  failure  was  not  detected.  Figure  B.54  and  Figure  B.55 
show  false  alarm  occurrences  on  the  no  failure  filter  and  on  the  pitch  rate  filter. 
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The  same  types  of  results  were  obtained  when  soft  failure  was  tested  for  the 
airspeed  velocity  sensor  as  seen  in  Figures  B.56  -  B.58. 
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V.  CONCLUSIONS  AND 
RECOMMENDATIONS 

A.  CONCLUSIONS 

The  results  obtained  were  very  encouraging  for  the  detection  of  hard  failures. 
The  filters  were  well  tuned  to  detect  failures  under  those  particular  situations.  The 
problematic  results  obtained  when  simulating  soft  failures  were  anticipated.  These 
difficulties  have  also  been  encountered  by  other  researchers  working  in  the  field.  More 
work  will  be  required  to  render  this  feature  operational  for  the  present  FDI  algorithm. 

B.  RECOMMENDATIONS 

The  Kalman  gains  should  be  precalculated  and  stored  prior  to  being  used  since 
they  do  not  vary  with  measured  data. 

The  work  done  in  this  thesis  did  not  address  the  task  of  reconfiguration.  If 
the  FDI  algorithm  is  to  be  implemented  on  the  Bluebird  this  part  will  need  to  be 
addressed. 

The  whiteness  tests  described  in  chapter  III.  should  also  be  conducted  once 
the  algorithm  is  installed  on  the  test  bed  since  the  noise  experienced  in  the  working 
model  might  be  quite  different  from  that  which  what  was  generated  numerically  in 
the  Matlab  code. 

A  dual  failure  detection  scheme  could  be  implemented  in  future  development  in 
order  to  detect  several  failures  occurring  at  the  same  time. 

The  implementation  methods  from  [Ref.  19,  p.192-260]  should  be  investigated 
in  order  to  improve  the  robustness  of  the  algorithm  when  small  numbers  are  generated 
by  the  filter,  thus  eliminating  ill-conditioned  situations  inside  the  algorithm. 
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The  algorithm  implemented  was  tested  on  a  model  derived  from  the  linearization 
of  a  nonlinear  system  around  one  nominal  flight  condition.  More  tests  should  be 
conducted  using  models  linearized  around  several  flight  conditions. 
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APPENDIX  A:  USER’S  MANUAL 

This  appendix  describes  in  detail  the  steps  required  to  open  and  run  the  software 
which  generates  the  various  probability  charts  to  verify  if  failures  have  been  detected. 

The  user  must  be  familiar  with  the  basic  operation  of  the  Matlab  software 
package.  Additionally,  it  is  assumed  that  the  user  is  already  logged  on  to  a  MaTLAB 
capable  Unix  work  station.  Before  entering  the  MATLAB  environment,  one  must 
change  the  working  directory  to  the  one  which  contains  all  the  '‘.m’’  files,  in  this  case, 
“marioth”.  The  command  is: 

cd  marioth 

If  the  user  is  remotely  logged  onto  a  work  station,  he  must  set  the  DISPLAY  environ¬ 
ment  variable  appropriately  in  order  to  display  graphics.  The  command  which  sets 
this  variable  to  intrepid,  a  Sparc  2  work  station  in  the  Avionics  Lab  is: 

setenv  DISPLAY  intrepid.aa.nps.navy.mil:0 

Now  it  is  time  to  begin  the  MATLAB  session  by  typing: 

matlab 

The  next  command  to  run  the  main  program  is: 

fdi 

A  series  of  windows  will  appear  for  the  user  to  select  the  type  of  failure  to  be 
simulated.  The  user  is  required  to  position  the  arrow  of  the  mouse  on  the  pad  of  his 
choice  and  click  the  left  button  of  the  mouse  to  select  the  item.  The  first  window  to 
appear  is  for  the  selection  of  the  type  of  failure  to  be  simulated.  The  choices  are: 
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•  no  failure 

•  Haxd  or  abrupt  failure 

•  Soft  or  incipient  failure 

The  second  window  to  appear,  if  a  selection  other  than  "no  failure"  has  been 
chosen  from  the  previous  window,  is  for  the  selection  of  a  failure  source.  The  choices 
are: 

•  Actuator  Failure 

•  Sensor  Failure 

The  last  window  to  appear  will  be  to  either  select  an  actuator  type  or  a  sensor 
type,  depending  on  the  choice  made  from  the  previous  window.  If  the  selection  was 
for  an  actuator  failure,  the  choices  will  be: 

•  Elevator 

•  Aileron 

•  Rudder 

•  Thrust  Actuator 

If  the  selection  was  for  a  sensor  failure,  the  choices  will  be: 

•  u 

•  v 

•  w 

•  p 

•  q 

•  r 

•  phi 

•  theta 

•  psi 
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Before  running  the  program,  the  user  might  want  to  change  some  parameters 
inside  the  program  in  order  to  save  time  or  to  look  at  more  graphics. 

The  structure  of  the  FDI  program  will  now  be  presented  in  order  to  give  the 
user  insight  into  how  the  algorithm  works  prior  to  changing  the  values  inside  the 
different  files.  The  structure  of  the  algorithm  is  made  of  several  Matlab  files  and 
functions. 

The  main  file  is  called  fdi.m  and  the  majority  of  the  calls  to  the  different 
functions  are  initiated  within  this  file. 

The  first  file  to  be  accessed  is  ABCU_load  .m.  The  values  for  the  plant  matrices 
A,B,C.u  are  stored  in  this  file  along  with  the  initial  value  for  x(0/  —  1).  P(k/k  —  1) 
and  the  values  for  the  standard  deviation  of  the  process  noise  and  measurement  noise. 
If  many  runs  were  to  be  conducted  for  different  linearized  models  around  operating 
points,  the  plant  matrices  would  be  changed  in  this  file.  The  values  for  the  time 
increment  dt  as  well  as  the  Stop  time  can  be  changed  appropriately  to  vary  the 
total  number  of  observations. 

The  second  file  accessed  by  fdi  .m  is  the  function  f  aultchoice  .m.  This  function 
brings  up  the  various  pop-up  menus  and  allows  the  user  to  simulate  a  certain  fault 
in  the  plant  by  zeroing  the  column  or  changing  the  noise  on  certain  matrices.  No 
changes  are  required  from  the  user  to  this  file  unless  the  structure  of  the  program 
needs  to  be  changed. 

The  third  step  combines  the  calls  to  the  function  kalmn_gain.m  in  order  to 
calculate  the  gain  for  each  of  the  Kalman  filters  associated  with  a  particular  fault. 
Since  the  gains  are  independent  of  the  measurement  data,  once  the  program  has  been 
run  once  and  the  gains  are  stored  in  memory,  the  flag  “need_to_calculate_gains” 
can  be  set  to  “0”  and  some  time  will  be  saved  for  the  future  runs  since  the  gains  will 
not  be  recalculated  every  time. 
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The  fourth  step  comprises  the  calls  to  the  function  kalman.i  .m.  This  function 
returns  values  for  the  residuals  and  the  state  estimates.  Modifications  to  this  file 
can  be  made  under  the  heading  “measurements'1 .  where  a  step  input  of  one  second  is 
forced  into  the  plant.  The  size  of  the  step  input  is  set  to  five  degrees  for  the  control 
surfaces  and  to  50%  for  the  thrust  actuator.  A  call  to  the  function  kalman_plots 
is  located  at  the  bottom  of  the  file  and  can  be  enabled  by  removing  the  at  the 
beginning  of  the  line.  The  kalman_plots  function  generates  plots  for  the  process 
noise,  measurement  noise,  Kalman  gains,  error  covariance  matrices  P(k/k).  estimates 
vs.  noisy  measurements,  and  plots  for  the  residuals. 

The  ‘‘last  call”  of  the  main  file  is  to  the  function  pk.m.  This  function  calculates 
the  hypothesis  conditional  probability  pk  as  the  probability  that  a  measured  param¬ 
eter  assumes  the  value  at  k  conditioned  on  the  observed  measurement  history  up  to 
time  k.  It  also  generates  the  plots  to  show  the  probabilities  of  failure  associated  with 
each  sensor  and  actuator.  No  changes  are  required  from  the  user  to  this  file  unless 
the  structure  of  the  program  needs  to  be  changed. 
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Figure  B.3:  Transpose  Values  of  the  Covariance  Matrix  P  for  State  u 
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Figure  B.4:  Transpose  Values  of  the  Covariance  Matrix  P  for  State  v 
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Figure  B.5:  Transpose  Values  of  the  Covariance  Matrix  P  for  State  w 


Figure  B.6:  Transpose  Values  of  the  Covariance  Matrix  P  for  State  p 
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Figure  B.7:  Transpose  Values  of  the  Covariance  Matrix  P  for  State  q 
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Figure  B.8:  Transpose  Values  of  the  Covariance  Matrix  P  for  State  r 
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Figure  B.10:  Transpose  Values  of  the  Covariance  Matrix  P  for  State  theta 
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Figure  B.13:  Kalman  Gains  for  the  State  v 
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Figure  B.15:  Kalman  Gains  for  the  State  p 
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Figure  B.19:  Kalman  Gains  for  the  State  theta 
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Figure  B.20:  Kalman  Gains  for  the  State  psi 


M— Kirwnfl  mlmm  (-.)  yftall  and  updataa  yl 


Figure  B.21:  Measurement  and  State  Estimate  u 
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Figure  B.22:  Measurement  and  State  Estimate  v 
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Figure  B.23:  Measurement  and  State  Estimate  w 
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Figure  B.25:  Measurement  and  State  Estimate  q 
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Figure  B.26:  Measurement  and  State  Estimate  r 
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Figure  B.27:  Measurement  and  State  Estimate  phi 
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Figure  B.28:  Measurement  and  State  Estimate  theta 
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Figure  B.40:  Probability  of  Aileron  Hard  Failure 


Probability 


0  20  40  60  80  100  120  140  160  180  200 

obaatvaiton  index  (k) 

Figure  B.41:  Probability  of  Rudder  Hard  Failure 
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Figure  B.42:  Probability  of  Thrust  Actuator  Hard  Failure 
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Figure  B.43:  Probability  of  u  Sensor  Hard  Failure 
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Figure  B.44:  Probability  of  v  Sensor  Hard  Failure 
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Probability  of  w.  vertical  veiocty  hard  sensor  failure 
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Figure  B.45:  Probability  of  w  Sensor  Hard  Failure 
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Figure  B.46:  Probability  of  Roll  Rate  Sensor  Hard  Failure 
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Figure  B.47:  Probability  of  Pitch  Rate  Sensor  Hard  Failure 
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Figure  B.48:  Probability  of  Yaw  Rate  Sensor  Hard  Failure 


63 


ProtubMy  ot  prt,  rot  Mnaor  hard  taftira 


observation  index  (k) 

Figure  B.49:  Probability  of  Roll  Angle  Sensor  Hard  Failure 


Figure  B.50:  Probability  of  Angle  of  Attack  Sensor  Hard  Failure 
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Figure  B.51:  Probability  of  Heading  Angle  Hard  Failure 
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Figure  B.52:  Probability  of  No  Failure 
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Figure  B.55:  Probability  of  Soft  Failure  on  the  Elevator  Actuator 
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Figure  B.56:  Probability  of  Soft  Failure  on  u  sensor 
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APPENDIX  C:  FDI  PROGRAM  LISTING 

fdi.m 


7,  Fault  detection  and  isolation  for  the  Bluebird  test  bed  aircraft 
7. 

%  By:  Capt  M.  Levesque 
7.  Canadian  Air  Force 

7. 

7.  Main  file  in  thesis  for  the  partial  fulfillement  of 
7.  the  requirement  for  the  degree  of  MSEE 

7. 

7%  This  is  the  main  file  that  call  all  the  subroutines 
7.required  for  the  fdi  algorithm 

7. 

clear 

clg 

elf 


ABCU_load  7.1oads  nominal  matrix  values  for  aircraft,  A,B,C  and  u 

kmax=Stop_time/dt ; 

7.  Plant  matrix 
[rC,cC]=size(C) ; 

CrA,cA]=size(A) ; 

^initialisation  of  input  matrices  from  selected  failure 


[Bm,  Cm, Wm,Vm] “fault choice (B,C,Wik,Vik, incipient _fault_noise_f actor) ; 


7.noise  generation 

•/  V  V  •/•/  •/  •/  V  •/  V  V  V  •/•/•/♦/  •/ 


w  n  n  n  n  n  n  n n  n  n  n 


TfT/T/T/T/T 


VNNNNh 


Qm=zeros(rA,  (rA*(kmax))) ;  7.covariance  of  process  noise 
Rm“zeros(rC, (rC*(kmax))) ;  7.covariance  of  measurement  noise 


wm=randn(rA,kmax) ;  7.randn(M,N)  is  an  M  by  N  matrix  with  random 
vm=randn(rC,kmax) ;  7.entries  normally  distributed,  mean  0.0  var  1. 

7cov(X)  is  covariance  matrix  of  X  when  each  row  is  an  observation  and 
Xeach  column  a  variable 

Qm=cov(wm');  7.unweighted  covariance  matrix  of  the  process/plant  noise 
Rm-cov(vm');  ^unweighted  covariance  matrix  of  the  measurement  noise 
[phidummy , GAMMA wm]  =c2d (A , Wm , dt ) ;  7.gives  phi(k+l,k)  and  Gammaw(k) 

7.calls  to  calculate  gains  covariances  of  error  matrices 
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need_to_calculate_gains»l 
if  need_to_calculate_gains==l 


ftKalman  filter  tuned  for  no  failure; 

Blkf=B; 

Clkf =C ; 

[G1 ,Pkkl ,al]=kalmn_gain(P_init , Vik,Wik,A,Blkf ,Clkf ,dt ,Stop_time) ; 


‘/.hard  actuator  failures 

mxtmmximtmm 


'/.Kalman  filter  tuned  for  the  system  with  elevator  hard  failure; 
B2kf =B*diag( [0  111]); 

C2kf=C; 

[G2,Pkk2,a2]=kalmn_gain(P_init ,Vik,Wik,A,B2kf ,C2kf ,dt ,Stop_time) 


'/.Kalman  filter  tuned  for  the  system  with  aileron  hard  failure; 
B3kf=B*diag( [1  0  1  1]); 

C3kf=C; 

[G3 JPkk3,a3]=kaImn_gain(P_init , Vik,Wik,A,B3kf ,C3kf ,dt ,Stop_time) ; 


'/.Kalman  filter  tuned  for  the  system  with  rudder  hard  failure; 

B4kf =B*diag( [1  101]); 

C4kf=C; 

[G4 ,  Pkk4 ,  a4]  =kalmn_gain  (P_init , Vik , Wik , A ,  B4kf ,  C4kf ,  dt ,  Stop_t  ime) ; 


'/.Kalman  filter  timed  for  the  system  with  thrust  actuator  hard  failure; 
B5kf =B*diag( [1  110]); 

C5kf =C ; 

[G5 , Pkk5 , a5] =kalmn_gain (P_init , Vik , Wik , A , B5kf , C5kf , dt , Stop_t ime) ; 
'Xhard  sensor  failures 


mmmmmmmx/.*/. 


'/.Kalman  filter  tuned  for  the  system  with  u,  forward  velocity 
hard  failure; 


B6kf=B; 

C6kf =C*diag( [0  11111111]); 

[G6 , Pkk6 , a6] »kalmn_gain (P_ init , Vik , Wik , A , B6kf , C6kf , dt , Stop_t ime) ; 


’/.Kalman  filter  tuned  for  the  system  with  v,  lateral  velocity 
hard  failure; 

B7kf =B ; 

C7kf=C*diag( [1  01111111]); 
[G7,Pkk7,a7]=kalmn_gain(P_init ,Vik,Wik,A,B7kf ,C7kf ,dt ,Stop_time) ; 

'/.Kalman  filter  tuned  for  the  system  with  w,  vertical  velocity 
hard  failure; 

B8kf=B; 

C8kf=C*diag([l  10111111]); 

[G8 , Pkk8 , a8] =kalmn_gain (P_init , Vik , Wik , A , B8kf , C8kf , dt , Stop_t ime) ; 
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XKalman  filter  tuned  for  the  system  with  p,  roll  rate  hard  failure; 

B9kf =B ; 

C9kf=C*diag( [1  11011111]); 

[G9 ,Pkk9 ,a9] =kalmn_gain(P_init , Vik.Wik ,A,B9kf ,C9kf ,dt ,Stop_time) ; 

'/.Kalman  filter  tuned  for  the  system  with  q,  pitch  rate  hard  failure; 
B10kf=B; 

ClOkf =C*diag( [1  11101111]); 

[G10 ,PkklO , alO]=kalmn_gain(P_mit , Vik, Wik,A,B10kf  ,C10kf ,dt  ,Stop_time) ; 

‘/.Kalman  filter  tuned  for  the  system  with  r,  yaw  rate  hard  failure; 

Bllkf =B ; 

Cllkf =C*diag( [1  11110111]); 

[Gil  .Pkkll , all]  =kalmn_gain(P_init , Vik.Wik ,  A, Bllkf  , Cllkf  ,dt  ,Stop_time) ; 

*/, Kalman  filter  tuned  for  the  system  with  phi,  roll  sensor  hard  failure; 
B12kf=B; 

C12kf =C*diag( [1  11111011]); 

[G12,Pkkl2,al2] =kalmn_gain(P_init ,Vik,Wik,A,B12kf ,C12kf ,dt ,Stop_time) ; 

'/Kalman  filter  tuned  for  the  system  with  theta,  angle  of  attack  sensor 
hard  failure; 

B13kf =B ; 

C13kf=C*diag( [1  1  1  1  1  1  1  0  1]); 

[G13,Pkkl3,al3]=kalmn_gain(P_init ,Vik,Wik,A,B13kf ,C13kf ,dt ,Stop_time) ; 

'/.Kalman  filter  tuned  for  the  system  with  psi,  heading  sensor 
hard  failure; 

B14kf=B; 

C14kf =C*diag( [1  11111110]); 

[G14,Pkkl4,al4]=kalmn_gain(P_init ,Vik,Wik,A,B14kf ,C14kf ,dt ,Stop_time) ; 


end;  '/.for  if  need_to_calculate_gains 


'/.Calls  to  kalman  filter  function  to  calculate  residue  and  state 


estimates 

mmxmxmmxxxxxmmmmxmmxmmxxmmmmmxmx 


%the  nominal  system 

xxxxxxxxxxxxxxxxxxxxxxx 

'/.Kalman  filter  tuned  for  no  failure; 

[resl ,xhatl]«kalman_i(Gl ,Pkkl ,xk_init ,GAMMAwm,Vm,wm,vm,A,Bm,Cm,u,Blkf , 
Clkf ,dt ,Stop_time) ; 


Xhard  actuator  failures 

y.y.y.r/.y.'/.y.y.y.y.y.y.y.y.'/.'/.'/.'/.'/.'/,'/.'/. 


'/.Kalman  filter  tuned  for  the  system  with  elevator  hard  failure; 

[res2 , xhat  2] =kalman_ i (G2 , Pkk2 , xk_ init , GAMMA wm , Vm , wm , vm , A , Bm , Cm , u , B2kf , 
C2kf , dt , Stop_t ime) ; 
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'/.Kalman  filter  tuned  for  the  system  with  aileron  hard  failure; 

[res3 , xhat3] =kalman_ i (G3 , Pkk3 , xk_ init , GAMMA wm , Vm , vm , vm , A , Bm , Cm , u , B3kf , 
C3kf ,dt ,Stop_time) ; 

'/Kalman  filter  tuned  for  the  system  with  rudder  hard  failure; 

[res4 , xhat4] =kalman_ 1  (G4 , Pkk4 , xk.init , GAMMA wm , Vm , wm , vm , A , Bm , Cm , u , B4kf , 
C4kf ,dt ,Stop_time) ; 


'/.Kalman  filter  tuned  for  the  system  with  thrust  actuator  hard  failure; 
[res5 , xhat5] =kalman_ i (G5 , Pkk5 , xk_ init , GAMMA wm , Vm , wm , vm , A , Bm , Cm , u , B5kf  , 
C5kf , dt , Stop_t ime) ; 


'/.hard  sensor  failures 

xxxxxxxxxxxxxxxxxxxxxxx 


'/.Kalman  filter  tuned  for  the  system  with  u,  forward  velocity  hard 
failure; 

[res6 , xhat6] =kalman_ i (G6 , Pkk6 , xk_ init , GAMMA wm , Vm , wm , vm , A , Bm , Cm , u , B6kf , 
C6kf ,dt ,Stop_time) ; 


'/.Kalman  filter  tuned  for  the  system  with  v,  lateral  velocity  hard 
failure; 

[res7 , xhat7 ] =kalman_ i (G7 , Pkk7 , xk_ init , GAMMA wm , Vm , wm , vm , A , Bm , Cm , u , B7kf , 
C7kf ,dt ,Stop_time) ; 

'/.Kalman  filter  tuned  for  the  system  with  w,  vertical  velocity  hard 
failure; 

[res8 , xhat8] =kalman_ i  (  G8 , Pkk8 , xk_ init , G AMMAwm , Vm , wm , vm , A , Bm , Cm , u , B8kf , 
C8kf ,dt ,Stop_time) ; 

'/.Kalman  filter  tuned  for  the  system  with  p,  roll  rate  hard  failure; 
[res9 , xhat9] =kalman_ i  (G9 , Pkk9 , xk_ init , GAMMAwm , Vm , wm , vm , A , Bm , Cm , u , B9kf , 
C9kf ,dt ,Stop_time) ; 

‘/Kalman  filter  tuned  for  the  system  with  q,  pitch  rate  hard  failure; 
[reslO ,xhatlO] =kalman_i(G10 ,PkklO ,xk_init ,GAMMAwm,Vm,wm,vm,A,Bm,Cm,u, 
BlOkf ,C10kf  ,dt  ,Stop_time) ; 


'/Kalman  filter  tuned  for  the  system  with  r,  yaw  rate  hard  failure; 
[resll,xhatll]=kalman_i(Gll ,Pkkll ,xk_init ,GAMMAwm,Vm,wm,vm,A,Bm,Cm,u, 
Bllkf ,Cllkf ,dt,Stop_time) ; 

'/.Kalman  filter  tuned  for  the  system  with  phi,  roll  sensor  hard  failure; 
[resl2,xhatl2] =kalman_i(G12 ,Pkkl2 ,xk_init ,GAMMAwm,Vm,wm,vm,A,Bm,Cm,u, 
B12kf ,C12kf  ,dt,Stop_time) ; 

'/.Kalman  filter  tuned  for  the  system  with  theta,  angle  of  attack 
sensor  hard  failure; 

[resl3,xhatl3] =kalman_ i (G 13 , Pkk 13 , xk_ init , GAMMAwm , Vm , wm , vm , A , Bm , Cm , u , 
B13kf ,C13kf  ,dt ,Stop_time) ; 

'/Kalman  filter  tuned  for  the  system  with  psi,  heading  sensor  hard 
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failure; 

[res 14 , xhat 14] =kalman_ i (G14 , Pkkl4 , xk_init , GAMMA wm , Vm , wm , vm , A , Bm , Cm , u , 
B14kf ,C14kf ,dt ,Stop_time) ; 


%Soft  or  incipient  failure 


mmmmxmx  mmm 


'/.Calls  to  probability  function  to  calculate  the  probabilities  on 
each  filter 


pkx=pk(resl ,al ,res2 ,a2 ,res3,a3 , res4, a4 ,res5 , a5 ,res6,a6,res7 ,a7 ,res8 ,a8, 
res9,a9,resl0,al0,resll .all , res 12 ,al2,resl3,al3,resl4,al4,dt ,Stop_time) ; 
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ABCUioad.m 


'/.this  m  file  is  executed  at  the  beginning  of  the  fdi  main  file  to  load 
Xthe  memory  with  the  following  initial  conditions 

Stop_time=2; 
dt=0 .01 ; 


'/.verification  with  model  in  the  book  by  Candy  p.127 


A®8 [0.999  0.0 
0  1]  ; 
B-[l 
0]; 


u*0; 

C=[29 .8  -0.623 
0  24.9]; 

xk  init=[2.5 
2.5]; 

P_init=le-6; 

Wik=[sqrt(l0)  sqrt(10)]J; 
Vik=[sqrt (5 .06e4)  sqrt(l .4e5)] ’ ; 


'/.verification  with  model  in  the  book  by  Andrews  and  Grewal  p.143 

xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 

A=  [0  1 

-25  -2*. 2*5]; 

B=  [0 

12]; 


u*l; 


C=[l  0]  ; 
xk_init=[0 

0]; 


P_init=2; 

Wik=[0  sqrt (4.47)]  ’ ; 

Vik=[sqrt(0.01)]  ’ ; 

incipient _fault_noise_factor=5 ; 


'/(model  simulation  using  numbers  from  Dave  Kuechenmeister'  s  thesis 

xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 

A=  [-0.0635  0  0.3277  0  -1.4922  0  0  -32.1740 

0  -0.3911  0  1.6086  0  -72.6109  32.1740  0 


-0.7572  0  -4.7741  0  67.9934 

0  -0.1471  0  -5.4414  0  1.5183  0 

0.0151  0  -0.1933  0  -3.1672  0  0 

0  0.1440  0  -1.0578  0  -0.8114  0 

0  0  0  1  0  0  0 

0  0  0  0  1  0  0 

0  0  0  0  0  1  0 


'/. 

B»[  -4.5835 


0  -0.0002  -0.0002 
0 
0 
0 
0 
0 
0 


0 

0 

0 

0 

0 

0 

0 

0 

0] 


0  8.7745 
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0  5.8282  0 

-38.8681  0  0 

0  0.6252  47.1717 

-22.0417  0  0 

0  -7.3151  -6.4345 
0  0  0 
0  0  0 
0  0  0 


•/.deterministic  control  input 

u=[  -0.0181 
0 
0 

0 . 2336] ; 

C=eye(length(A) ) ; 

xk_init= [73 . 3000 
0.0 
0.0 
0.0 
0.0 
0.0 
0.0 
0.0 
0.0]  ; 

P_init=l; 

Wik=[sqrt(0. 1)  sqrt(O.Ol)  sqrt (0 . 01)  , .  .  . 

sqrt (0.001)  sqrt (0.001)  sqrt (0 .001) ,.. . 

sqrt (0.001)  sqrt (0.001)  sqrt (0 .001)]  '  ;  */.Ww=  [sigma  .  .  .  .] 

Vik= [sqrt (10)  sqrt(l)  sqrt(l)  sqrt(0.057)  sqrt(0.057)  sqrt (0 . 057)  ,.. . 
sqrt (0.2*pi/ 180)  sqrt (0 . 2*pi/180)  sqrt (3*pi/l80)] ' ; 

incipient_f ault_noise_f actor=5 ; 


•/model  simulation  using  numbers  from  Eric  Hallberg's  thesis 

mnyfflmmmmmmmmmmmnnm 


A= [-0.0825  0.1193  0.0467  0.0011  1 

-0.1020  -0.4281  -0.0059  -1.4341 
-0.8561  -0.0427  -5.2818  -3.4481  75 
0.0048  -0.1583  0.0149  -6.0885  0 

0.0069  -0.0039  -0.2155  -0.1305  -3 
-0.0061  0.1680  -0.0002  -0.8975  -0 

000  1.0000  -0 
0  0  0  0  0 
0  0  0  0  0 

*/. 


3303  3.4789  0.0027  -32.0613  0 

0  -80.7651  30.8814  0.7255  0 

6252  0.0006  -8.4582  2.5534  0 

3198  1.6817  -0.0074  0.0022  0 

5224  0.0441  0.1064  -0.0321  0 

0086  -0.9863  0.0002  0  0 

0226  -0.0811  -0.0001  0.0971  0 

9632  -0.2687  -0.0965  0  0 

2696  0.9666  0.0016  -0.00820]; 


75 


B* [-7. 5739  0  -0.3370  8.7745 

-0.2898  0  7.2176  0 

-47.9100  0  -1.4904  0 

1.8944  58.1206  0.4870  0 

-27.2996  1.5561  0.0352  0 

-0.0418  -10.8643  -9.0965  0 

0  0  0  0 

0  0  0  0 

0  0  0  0]  ; 

'/.deterministic  control  input 
u=[  0 
0 
0 

•  75]  ; 

C=eye(length(A)) ; 

xk_init= [81 . 5317 
3.5119 
-1.4341 
0 . 0042 
0.0275 
0.0925 
0.2720 
-0 . 0840 
1.7317] ; 

P_init=l ; 
wf=l .5; 

Wik=[l*wf  0 . 5*wf  0 . 5*wf  0.057*wf  0.057*vf  0.4*wf  0 ,3*pi/180*vf 
0.3*pi/180*wf  l*pi/ 180*wf] ’ ; 

Vik=[l  .5  .5  0.57  0.57  0.57  0.5*pi/180  0.5*pi/180  3*pa/180] * ; 
incipient.f ault_noise_f actor=2 ; 


76 


faultchoice.m 


function  [Bm , Cm , Wm , Vm] =f ault cho ice (B , C , Wik , Vik , 
incipient.f ault_noise_f actor) 

y. 

X  This  function  modifies  the  values  of  the  matrices  A,  B,  C,  or  u 
'/.  according  to  the  type  of  failure  selected  from  the  user. 

!% 

X 

y. 


Xchoice  of  failure  to  be  simulated 

xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 

Failure_type=menu( 'Choose  a  fault  scenario  to 


test  the  FDI  algorithm’ , 


’no  failure' ,  ... 

'Hard  or  abrupt  failure’ , 
'Soft  or  incipient  failure’); 


if  Failure.type  "=  1 

Failure_source=menu( 'Choose  a  failure  source’,  ... 

'Actuator  Failure’,  ... 

'Sensor  Failure’); 
if  Failure_source  ==  1 

Actuator_type=menu(’ Choose  a  control  surface  related  to  your 
actuator  failure’ ,  ... 

’Elevator’ ,  ... 

’Aileron’,  ... 

’ Rudder ' ,  ... 

'thrust  actuator’); 
elseif  Failure_source  ==  2 

Sensor_type=menu(’ Choose  a  parameter  related  to  your 

sensor  failure',  ... 

'u' ,  ... 

'v',  ... 

'w' ,  ... 

'P\  ... 

’q\  ... 

’r\  ... 

’phi',  ... 

'theta' ,  ... 

'psi') ; 

end; 

end; 

end; 


xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 

^implementation  of  selected  failure  to  input  matrices 

xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 

Xno  failures 

•/.xxxxxxxxxxxxy.y.xy.y.y.y. 
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if  Failure.type  ==  1 
Bm=B ; 

Cm*C  j 
Wm=Wik; 

Vm»Vik; 

'/.Hard  or  abrupt  failures 

mmxmxmmmmx 

elseif  Failure.type  ==  2 
if  Failure_source  ==  1 
Wm=Wik ; 

Vm*Vik; 


Cm*C; 

if  Actuator_type  ==  1 
Bm=B*diag([0  1  1  1]); 
elseif  Actuator.type  == 
Bm=B*diag([l  0  1  1]); 
elseif  Actuator.type  == 
Bm=B*diag([l  1  0  1]); 
elseif  Actuator_type  == 
Bm*B*diag([l  1  1  0] ) ; 
end; 


2 

3 

4 


'/.actuator  hard  failure 

*/, elevator  hard  failure 
'/.aileron  hard  failure 
X rudder  hard  failure 
'/.Thrust  actuator  hard  failure 


elseif  Failure_source  ==  2  '/.sensor  hard  failure 
Wm=Wik; 

Vm=Vik; 


Bm=B; 


if  Sensor_type  ==  1 

Cm=C*diag([0  1111111 
elseif  Sensor_type  ==  2 

Cm=C*diag([l  0111111 
elseif  Sensor.type  ==  3 

Cm=C*diag( [1  1011111 
elseif  Sensor_type  ==  4 

Cm=C*diag( [1  1101111 
elseif  Sensor.type  ==  5 

Cm=Odiag([l  1110  111 
elseif  Sensor.type  ==  6 

CmaC*diag([l  1111011 
elseif  Sensor.type  ==  7 


1]) 

1]) 

13) 

13) 

13) 


*/,u,  forward  velocity 

9 

'/,v,  lateral  velocity 

9 

Xw,  vertical  velocity 

9 

*/,p,  roll  rate 

) 

'/,q,  pitch  rate 

9 

i, r,  yaw  rate 


13); 


'/iphi,  roll  sensor 


Cm=C*diag([l  1111101  1]); 
elseif  Sensor.type  ==  8'/,theta,  angle  of  attack  sensor 
Cm=C*diag([l  1111110  13); 
elseif  Sensor.type  ==  9  '/.psi,  heading  sensor 

Cm=C*diag( [1  11111110]); 


end; 

end; 


hard 

hard 

hard 

hard 

hard 

hard 

hard 

hard 

hard 


failure 

failure 

failure 

failure 

failure 

failure 

failure 

failure 

failure 


'/.Soft  or  incipient  failure 
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elseif  Failure.type  ==  3 
nf=incipient_f ault.no ise_f actor ; 

if  Failure.source  ==  1  '/actuator  soft  failure 
Bm=B; 

Cm=C; 

Wm=Wik; 

Vm=Vik; 

if  Actuator.type  ==  1  '/.elevator  hard  failure 

Bm*B*diag( [nf  1  1  1]); 

elseif  Actuator.type  ==  2  /aileron  hard  failure 

Bm=B*diag( [1  nf  1  1]); 

elseif  Actuator.type  ==  3  /rudder  hard  failure 

Bm=B*diag( [l  1  nf  1]); 

elseif  Actuator.type  ==  4  '/Thrust  actuator  hard  failure 

Bm=B*diag([l  1  1  nf 3 ) ; 
end; 

elseif  Failure.source  ==  2  '/.sensor  soft  failure 
Bm=B; 

Cm=C; 

Wm=Wik; 

if  Sensor.type  ==  1  Xu,  forward  velocity  hard  failure 

Vm=diag([nf  1111111  l])*Vik; 
elseif  Sensor.type  ==  2  Xv,  lateral  velocity  hard  failure 
Vm=diag([l  nf  1  1  1  1  1  1  l])*Vik; 
elseif  Sensor.type  ==  3  /w,  vertical  velocity  hard  failure 

Vm=diag([l  1  nf  1  1  1  1  1  l])*Vik; 
elseif  Sensor.type  ==  4  */.p,  roll  rate  hard  failure 

Vm=diag( [1  1  1  nf  1  1  1  1  l])*Vik; 
elseif  Sensor.type  ==  5  Xq,  pitch  rate  hard  failure 
Vm=diag( [1  1  1  1  nf  1  1  1  l])*Vik; 
elseif  Sensor.type  ==  6  Xr,  yaw  rate  hard  failure 
Vm=diag([l  1  1  1  1  nf  1  1  l])*Vik; 
elseif  Sensor.type  ==  7  Xphi,  roll  sensor  hard  failure 
Vm=diag( [1  1  1  1  1  1  nf  1  l])*Vik; 
elseif  Sensor.type  ==8'/, theta,  angle  of  attack  sensor  hard  failure 
Vm=diag( [1  1  1  1  1  1  1  nf  l])*Vik; 
elseif  Sensor.type  ==  9  Xpsi,  heading  sensor  hard  failure 
Vm=diag( [1  1111111  nf])*Vik; 
end; 

end; 

end; 
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kalmn-gain.m 


function  [G,Pkk,a]=kalmn_gain(P_init ,Vik,Wik,A,Bikf ,Cikf ,dt ,Stop_time) 

kmax=Stop_time/dt ; 

*/,  Plant  matrix 
[rC,cC]=size(Cikf ) ; 

[rA,cA]=size(A) ; 


'/.matrix  initialisation 

xxxxxxxxxxxxxxxxxxxxxxxxxxxxi 

Pkkml=zeros(rA,  (rA*kmax+l))  ; 
Pkk=zeros (rA , (rA* (km ax) ) ) ; 

G=zeros(rA,  (rC*(kmax)) ) ; 
a=zeros(rC, (rC*(kmax))) ; 


wxxxxxxxxxxxxxxxxxxxxx 

y.p(k/k-D 

*/,P(k/k)  covanamce  of  estimation 
'/.error  matrix 

'/.Kalman  Gains 

'/.hypothesized  filter's  internally 
'/.computed  residual  covariance 


'/.noise  generation 


xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 


Qik=zeros(rA,  (rA*(kmax) ))  ;  V.covariance  of  process  noise 

Rik=zeros(rC,  (rC*(kmax))) ;  ’/.covariance  of  measurment  noise 


w=randn(rA,kmax) ;  '/.randn(M,N)  is  an  M  by  N  matrix  with  random 
v=randn(rC,kmax) ;  '/.entries  normally  distributed,  mean  0.0  var  1. 

’/.cov(X)  is  covariance  matrix  of  X  when  each  row  is  an  observation  and 
Xeach  column  a  variable 

Qik=cov(w’) ;  ^unweighted  covariance  matrix  of  the  process/plant  noise 
Rik=cov(v');  '/.unweighted  covariance  matrix  of  the  measurement  noise 
[phidummy,GAMMAik]=c2d(A,Wik>dt)  ;  '/.gives  phi(k+l,k)  and  Gammaw(k) 


X 


KALMAN  FILTER  CALCULATION 


xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx xxxx 


[phi.ikf ,del_ikf]=c2d(A,Bikf ,dt) ;  ’/.gives  phi(k+l,k)  and  del(k) 


'/,  Offline  calculations,  generation  of  gain  schedule 
X  (The  Gains  do  not  depend  on  measurement  data) 


for  i=l : kmax 

'/.set  P(0/-l)=P_init ,  the  covariance  of  initial  state  value 
if  i==l , 

Pkkml(: , (i*rA-(rA-l) :i*rA))  =  ... 
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ones(rA.rA)  .*P_imt; 

end; 

'/^hypothesized  filter’s  internally  computed  residual  covariance  used 
in  G 

a(:,(i*rC-(rC-l):i*rC))  =  ... 

(  Cikf  *  Pkkml( : , (i*rA-(rA-l) :i*rA))  *  Cikf’  +  ... 
(Rik*diag(Vik))  ); 

'/.Kalman  Gains 

G( : , (i*rC-(rC-l) : i*rC) )  =  ... 

Pkkml ( : , (i*rA-(rA-l) : i*rA) )  *  Cikf’  *... 
inv(  a( : , (i*rC-(rC-l) : i*rC))  ); 

5iP(k/k)  ,  covariance  of  estimation  error  matrix 
Pkk(: , (i*rA-(rA-l) :i*rA))  =  (eye(rA)  -  ... 

G( : , (i*rC-(rC-l) : i*rC))  *  Cikf)  *  ... 

Pkkml ( : , (i*rA-(rA-l) : i*rA)) ; 

'/Cdiscrete  Lyapunov  equation 

'^update  the  estimation  error  covariance  matrix,  new  Pkkml  or  P(k+l/k) 
Pkkml ( : , ((i+l)*rA-(rA-l) : (i+l)*rA))  =  phi.ikf  *  ... 

Pkk( : , (i*rA-(rA-l) : i*rA))  *  phi_ikf’  +  ... 
(GAMMAik*GAMMAik’*Qik) ; 


end; 

'/,  m=mean(a’)  ; 

7,  tot_mean_a=mean(m) 
7.  co=cov(a(l, :)) 

7,  size  (a) 


81 


kalmanJ.m 


f unct ion  [residualk , xhatkk] =kalman_ i (G , Pkk , xk_ init , GAMMA wm , Vm , vm , vm , 


Bikf ,Cikf ,dt ,Stop_time) 


A .Bm.Cm.u, 


’/The  function  kalman_i 

Z 

/This  function  calculates  the  residuals  and  the  state  estimates  of 
Za  system  using  a  kalman  filter  tuned  according  to  the  input  values 
*/,in  above  bracket . 

Z 

/by  CAPT  M.  Levesque 
X  CAF 
’/  Nov  1993. 


x 


INITIALISATION  PART 


r«  liuunLunuun  rmu 

xxxxxxxxxxxxxxxxxxxxxxxxxx xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 

! unalias  rm 


X !rm  -r  kalman. data 
’/diary  kalman. data 


kmaLX=Stop_t  ime/dt ; 

X  Plant  matrix 
[rC,cC]=size(Cm) ; 
[rA,cA]=size(A) ; 


/matrix  initialisation 

//////////////// 


xk=zeros(rA,kmax+l) ; 
yk=zeros(rC,kmaLx) ; 
xhatkk=*zeros(rA,kmax) ; 
xhatkkml=zeros(rA,kmax+l) ; 
yhatk=zeros(rC,kmax) ; 
residualk=zeros(rC,kmaix)  ; 


/system  states 
/measurement  updates 
Zxhat(k/k)  state  estimates 
/xhat(k/k-l)  state  estimate  a-priori 
’/.measurement  estimates 
/residual  is  y  -  yhat 


KALMAN  FILTER  CALCULATION 


////////////////////X///////////////////////////////X////X//// /xxxxxxxx 


measurements 


///////////////////////////////////X///XXXXX//XXXXXX 


Cphi,del]=c2d(A,Bm,dt) ;  '/.gives  phi(k+l,k)  and  del(k) 

uinit=u; 

for  i=l:kmax, 

if  i<l/dt  '/,1  sec  unit  step  input 
u= [3*pi/180 
5*pi/180 
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3*pi/180 

l]; 

else 
u=uinit ; 
end 


if  i==l , 

xk(: ,i)=xk_init; 
end; 


xk(:,i+l)  =  phi  *  xk(:,i)  +  del 

yk(:,i)  =  Cm  *  xk(:,i)  +  diag(Vm) 


*  u  +  diag(GAMMAvm)  *  wm(:,i); 

*  vm( :  ,  i) ; 


end; 


*/. 


Online  calculations,  generation  of  estimates 

. 7  •/•/•/•/  V  •/•/•/  V  */•/•/•/•/•/•/•/•/•/  •/ 


[phi_ikf ,del_ikf]=c2d(A,Bikf ,dt) ;  '/.gives  phi(k+l,k)  and  del(k) 


for  i=l  :kmax 

if  i<l/dt  '/.l  sec  unit  step  input 
u=[3*pi/180 
5*pi/180 
3*pi/180 
l]; 
else 
u=uinit ; 
end 

’/.initial  states 
if  i==l , 

xhatkkml(: ,i)=xk_init; 
end; 

'/(measurement  estimate 

yhatk(:,i)  =  Cikf*xhatkkml( : ,i) ; 

*4innovation  process,  error  in  the  measurement  and  estimate 
residualk(: ,i)  =  yk( : , i)-yhatk( : ,i) ; 

/(state  estimate 

xhatkk(:,i)  =  xhatkkml( : , i)  +  ... 

G( : , (i*rC-(rC-l) : i*rC) )  *  ... 

(residualk( : ,i)) ; 

'/(State  estimate  update 

xhatkkml( : ,i+l)  =  phi.ikf  *  xhatkk(:,i)  +  del_ikf  *  u; 

end; 
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'/.diary  off 

kalman.plot  s ( A , Cm , G , Pkk , yhatk , yk , res idualk , wm , vm , dt , Stop_t  ime) 
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kalman-piots.m 


f unct ion  []  =kalman_plot s ( A , C , G , Pkk , yhatk , yk , res idualk ,  w ,  v ,  dt , St op_t ime) 
XThe  function  kalman.plots 

X 

'/.This  function  is  used  to  generate  plots  from  the  values  calculated 
'/.in  the  function  kalman.i. 

'/. 

Xby  CAPT  M.  Levesque 
X  CAF 
X  Nov  1993 . 

y. 

X  PLOTS 


X 

[rC,cC]=size(C) ; 
[rA,cA]=size(A) ; 

kmax=Stop_time/dt ; 

k=l  :kmax; 

'/.plot  of  the  noise 


producenoiseplot=0 ; 
if  producenoiseplot==l 

plot (k,w) 

title(’ Process  noise1) 

xlabel(’ observation  index  (k)’) 

ylabel( ’white  noise  (mean=0,  variance=l) ’ ) 

pause; 

plot(k,v) 

t it le( ’Measurement  noise’) 
xlabel( 'observation  index  (k)') 
ylabel(’white  noise  (mean=0,  variance=l) ’ ) 
pause 
end; 

Xplot  of  the  kalman  gains 


producegainplot=0 ; 
if  producegainplot==l 
k=l :kmax; 
for  i=l:rA 
for  r=l : rC 

plot(k,G(i , (k.*rC-(rC-r)))) 

pause; 

hold  on 
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end; 

titleCC' Kalman  Gain  for  the  state  x’ ,num2str(i) ,] ) 

xlabeK’ observation  index  (k)’) 

ylabelC 'Kalman  Gain  (G)') 

pause 

hold  off 


end; 

end; 

Xplot  of  the  covariance  matrix  Pkk 

xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 

producecovarianceplot=0 ; 
if  producecovarianceplot==l 
k=l : km ax ; 

for  i=l:rA 

plot (k , Pkk ( i , (k . *r A- (r A- i ) ) ) ) 

title( [’Covariance  of  estimation  error  matrix,  diagonal  values  for 
the  state  x’ ,num2str(i) ,] ) 
xlabeK 'observation  index  (k)’) 
ylabelC 'Covariance,  P(k/k)’) 
pause 
end; 

end; 


Xplot  of  the  measurements  suid  its  estimates 

XXXXXXXXXXXXXXXXXXXXXXXXXX7.XXXXXXXXX7.XXX7.X 


producemeasurements=0 ; 
if  producemeasurements==l 

k=l : (km ax) ; 
for  n=l:rC, 

plot (k , yhatk(n 
hold  on; 
plot(k,yk(n, :)) 

title([ 'Measurement  estimate  (-.)  yhat ' ,num2str(n) , '  and  updates 
y' ,num2str(n) ,]) 

xlabel(’ observation  index  (k)') 

ylabelC C’Y’ ,num2str(n) and  Yhat' ,num2str(n) ,]) 
pause ; 
hold  off; 
end; 
end; 

X  plot  of  the  residuals 

xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 


produceresidualplots=0 ; 
if  produceresidualplots==l 

for  n=l:rC, 

plot(k,residualk(n, : )) 
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title( [’Residuals  (y-yhat)  rk' ,num2str(n) ,] ) 
xlabel( 'observation  index  (k) ' ) 
ylabelC ['rk' ,num2str(n) ,] ) 
pause; 

end; 

end; 
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pk.m 

function  [pk]  *  pk(resl ,al,res2,a2,res3,a3,res4,a4>res5,a5,res6,a61 
res7 ,a7 ,res8 ,a8 ,res9 ,a9 , res  10, alO , res 11 .all , res 12 , al2 , res  13 , al3 , 
resl4,al4,dt ,Stop_time) 

•/. 

‘/.The  function  pk 

•/. 

XThis  function  calculates  the  hypothesis  conditional  probability  pk 
’/.as  the  probability  that  a  measured  parameter  assumes  the  value  at  k 
'/.conditioned  on  the  observed  measurement  history  up  to  time  k. 


kmax=Stop_time/dt ; 
[ra,ca]=size(al) ; 

pkl_kml=l ;  '/.pk( :  ,0)=1 ; 

pk2_kml=l ; 
pk3_kml=l ; 
pk4_kml=l ; 
pk5_kml=l ; 
pk6_kml=l ; 
pk7_kml=l ; 
pk8_kml=l ; 
pk9_kml=l ; 
pkl0_kml=l ; 
pkll_kml=l ; 
pkl2_kml=l ; 
pkl3_kml=l ; 
pkl4_kml=l ; 

min-  0.0001; 


'/.  case  for  k=l  to  have  a  value  for  p(t-l),  index  can  not  equal  zero! 


k-1; 

fzkl(k)  -It  ((2*pi)‘(ra*(l/2))*det(al(: , (k*ra-(ra-l) :k*ra) ) ) 
‘0.5)  *  ... 

exp(-0.5*resl( : ,k) ’*inv(al( : , (k*ra-(ra-l) :k*ra)))  *  ... 
resl( : ,k)  ) ; 

fzk2(k)  =  1  /  ((2*pi)‘(ra*(l/2))*det(a2(: , (k*ra-(ra-l) :k*ra))) 
‘0.5)  *  ... 

exp(-0.5*res2( : ,k) '*inv(a2( : , (k*ra-(ra-l) :k*ra)))  *  ... 
res2( : ,k)  ) ; 

fzk3<k)  =  1  /  ((2*pi)‘(ra*(l/2))*det(a3(: , (k*ra-(ra-l) :k*ra))) 
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*0.5)  *  ... 

exp(-0 . 5*res3( : ,k) '*inv(a3( : , (k*ra-(ra-l) :k*ra) ))  *  ... 
res3( : ,k)  ) ; 

fzk4(k)  =  1  /  ((2*pi) " (ra*(l/2))*det(a4( : , (k*ra-(ra-l) :k*ra))) 
*0.5)  *  ... 

exp(-0.5*res4( : ,k) '*inv(a4( : , (k*ra-(ra-l) :k*ra) ))  *  ... 
res4( : ,k)  ) ; 

fzk5(k)  =  1  /  ((2*pi)*(ra*(l/2))*det(a5(: , (k*ra-(ra-l) :k*ra))) 
*0.5)  *  ... 

exp(-0 . 5*res5( : ,k) ' *inv(a5( : , (k*ra-(ra-l) :k*ra) ))  *  ... 
res5( : ,k)  ) ; 

fzk6(k)  =  1  /  ( (2*pi) “ (ra*(l/2) )*det (a6( : , (k*ra-(ra-l) :k*ra) ) ) 
*0.5)  *  ... 

exp(-0.5*res6( : ,k) ’*inv(a6(: , (k*ra-(ra-l) :k*ra)))  *  ... 
res6( : ,k)  ) ; 

fzk7(k)  =  1  /  ((2*pi) ~(ra*(l/2))*det(a7( : , (k*ra-(ra-l) :k*ra))) 
*0.5)  *  . . . 

exp(-0.5*res7( : ,k) '*inv(a7( : , (k*ra-(ra-l) :k*ra) ))  *  ... 
res7 ( : ,k)  ) ; 

fzk8(k)  =  1  /  ( (2*pi) * (ra* (1/2) )*det (a8( : , (k*ra-(ra-l) :k*ra) ) ) 
*0.5)  *  ... 

exp(-0.5*res8(: ,k) ’ *inv(a8( : , (k*ra-(ra-l) :k*ra)))  *  ... 
res8(: ,k)  ); 

fzk9(k)  =  1  /  ((2*pi)*(ra*(l/2))*det(a9(: , (k*ra-(ra-l) :k*ra))) 
*0.5)  *  ... 

exp(-0.5*res9( : ,k) '*inv(a9( : , (k*ra-(ra-l) :k*ra)))  *  ... 
res9(:,k)  ); 

fzklO(k)  =  1  /  ((2*pi) " (ra*(l/2))*det(al0( : , (k*ra-(ra-l) :k*ra))) 
*0.5)  *  ... 

exp(-0 .5*resl0( : ,k) ’*inv(alO(: , (k*ra-(ra-l) :k*ra)))  *  ... 
reslO(:,k)  ); 

fzkll(k)  =  1  /  ((2*pi) * (ra*(l/2) )*det (all( : , (k*ra-(ra-l) :k*ra) )) 
*0.5)  *  ... 

exp(-0 .5*resll( : ,k) '*inv(all( : , (k*ra-(ra-l) :k*ra)))  *  ... 
resll( : ,k)  ) ; 

fzkl2(k)  =  1  /  ((2*pi)*(ra*(l/2))*det(al2(: , (k*ra-(ra-l) :k*ra))) 
*0.5)  *  ... 

exp(-0.5*resl2(: ,k) '*inv(al2(: , (k*ra-(ra-l) :k*ra)))  *  ... 
resl2( : ,k)  ) ; 

fzkl3(k)  =  1  /  ( (2*pi) * (ra*(l/2) )*det (al3( : , (k*ra-(ra-l) :k*ra) )) 
*0.5)  *  ... 
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exp(-0.5*resl3(: ,k) '*inv(al3(: , (k*ra-(ra-l) :k*ra)))  *  ... 
resl3( : ,k)  ) ; 


fzkl4(k)  =  1  /  ((2*pi)~(ra*(l/2))*det(al4(  :  ,  (k*ra-(ra-l)  :k*ra))) 
*0.5)  *  ... 

exp(-0.5*resl4(: ,k) ’*inv(al4(: , (k*ra-(ra-l) :k*ra)))  *  ... 
resl4( : ,k)  ) ; 

pkl (k) -f zkl (k) *pkl_kml/ . . . 

(fzk2(k)*pk2_kml  +  fzk3(k)*pk3_kml  +  fzk4(k)*pk4_kml  +  fzk5(k)*pk5_kml 
+  .  .  . 

fzk6(k)*pk6_kml  +  fzk7(k)*pk7_kml  +  fzk8(k)*pk8_kml  +  fzk9(k)*pk9_kml 
+  .  .  . 

fzklO(k)*pklO_kml  +  fzkll(k)*pkll_kml  +  fzkl2(k)*pkl2_kml  +  ... 
fzkl3(k)*pkl3_kml  +  fzkl4(k)*pkl4_kml  +  fzkl(k)*pkl_kml) ; 
if  pkl(k)<min,  pkl(k)=min;  end; 

pk2(k)=fzk2(k)*pk2_kml/ . . . 

(fzkl(k)*pkl_kml  +  fzk3(k) *pk3_kml  +  fzk4(k)*pk4_kml  +  fzk5(k) *pk5_kml 
+  .  .  . 

fzk6(k)*pk6_kml  +  fzk7(k) *pk7_kml  +  fzk8(k)*pk8_kml  +  fzk9(k) *pk9Jcml 
+  .  .  . 

f zklO (k) *pklO_kml  +  fzkll(k)*pkll_kml  +  fzkl2(k)*pkl2_kml  +  ... 
fzkl3(k)*pkl3_kml  +  fzkl4(k)*pkl4_kml  +  fzk2(k)*pk2_kml) ; 
if  pk2(k)<min,  pk2(k)=min;  end; 

pk3(k)=fzk3(k)*pk3_kml/ . . . 

(f zkl (k) *pkl_kml  +  fzk2(k)*pk2 _kml  +  fzk4(k)*pk4_kml  +  fzk5(k)*pk5_kml 
+  . . . 

f  zk6 (k) *pk6_kml  +  fzk7(k)*pk7_kml  +  fzk8(k)*pk8_kml  +  fzk9(k)*pk9_kml 

+  ... 

fzklO(k)*pklO_kml  +  fzkll(k)*pkll_kml  +  fzkl2(k)*pkl2_kml  +  . . . 
fzkl3(k)*pkl3_kml  +  fzkl4(k)*pkl4_kml  +  fzk3(k)*pk3_kml) ; 
if  pk3(k)<min,  pk3(k)=min;  end; 

pk4(k)=fzk4(k)*pk4_kml/. . . 

(f zkl (k) *pkl_kml  +  fzk2(k)*pk2_kml  +  fzk3(k)*pk3_kml  +  fzk5(k)*pk5_kml 
+  ... 

fzk6(k)*pk6Janl  +  fzk7(k)*pk7_kml  +  fzk8(k)*pk8_kml  +  fzk9(k)*pk9_kml 
+  ... 

fzklO(k)*pklO_kml  +  fzkll(k)*pkll_kml  +  fzkl2(k)*pkl2_kml  +  ... 
f zkl3 (k) *pkl3_kml  +  fzkl4(k)*pkl4_kml  +  fzk4(k)*pk4_kml) ; 
if  pk4(k)<min,  pk4(k)=min;  end; 

pk5(k)=fzk5(k)*pk5_kml/ . . . 

(fzkl(k)*pkl  Janl  +  f zk2(k)*pk2_kml  +  f zk3(k)*pk3_kml  +  fzk4(k)*pk4_kml 
+  ... 

f zk6 (k) *pk6_kml  +  fzk7(k)*pk7_kml  +  f zk8(k)*pk8_kml  +  fzk9(k)*pk9_kml 
+  ... 

f zklO (k) *pklO_kml  +  fzkll(k)*pkll_kml  +  fzkl2(k)*pkl2_kml  +  ... 
f  zkl3 (k) *pkl3_kml  +  fzkl4(k)*pkl4_kml  +  fzk5(k)*pk5_kml) ; 
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if  pk5(k)<min,  pk5(k)=min;  end; 


pk6(k)*fzk6(k)*pk6_kml/ . . . 

(fzkl(k)*pkl_kml  +  fzk2(k)*pk2_kml  +  fzk3(k) *pk3_kml  +  fzk4(k)*pk4_kml 
+  .  .  . 

fzk5(k)*pk5_kml  +  fzk7(k)*pk7_kml  +  fzk8(k)*pk8_kml  +  fzk9(k)*pk9_kml 
+  .  .  . 

fzklO(k)*pklO_kml  +  fzkll(k)*pkll_kml  +  fzkl2(k)*pkl2_kml  +  ... 
fzkl3(k)*pkl3_kml  +  fzkl4(k)*pkl4_kml  +  fzk6(k)*pk6_kml) ; 
if  pk6(k)<min,  pk6(k)=min;  end; 

pk7(k)=fzk7(k)*pk7_kml/. . . 

(fzkl(k)*pkl_kml  +  fzk2(k)*pk2_kml  +  fzk3(k)*pk3_kml  +  f zk4(k) *pk4_kml 
+  .  .  . 

fzk5(k)*pk5_kml  +  fzk6(k)*pk6_kml  +  fzk8(k)*pk8_kml  +  fzk9(k)*pk9_kml 
+  ... 

fzklO(k)*pklO_kml  +  fzkll(k)*pkll_kml  +  fzkl2(k)*pkl2_kml  +  ... 
fzkl3(k)*pkl3_kml  +  fzkl4(k)*pkl4_kml  +  fzk7(k)*pk7_kml) ; 
if  pk7(k)<min,  pk7(k)=min;  end; 

pk8(k)=fzk8(k)*pk8_kml/ . . . 

(fzkl(k)*pkl_kml  +  fzk2(k)*pk2_kml  +  fzk3(k)*pk3_kml  +  fzk4(k)*pk4_kml 
+  .  ,  . 

f zk5 (k) *pk5_kml  +  fzk6(k)*pk6_kml  +  fzk7(k)*pk7_kml  +  fzk9(k)*pk9_kml 
+  .  .  . 

f zklO (k) *pklO_kml  +  fzkll (k)*pkll_kml  +  fzkl2(k)*pkl2_kml  +  ... 
fzkl3(k)*pkl3_kml  +  fzkl4(k)*pkl4_kml  +  fzk8(k)*pk8_kml) ; 
if  pk8(k)<min,  pk8(k)=min;  end; 

pk9(k)=fzk9(k)*pk9_kml/ . . . 

(fzkl(k)*pkl_kml  +  fzk2(k)*pk2_kml  +  fzk3(k)*pk3_kml  +  fzk4(k)*pk4_kml 
+  .  .  . 

f zk5 (k) *pk5_kml  +  fzk6(k)*pk6_kml  +  fzk7(k)*pk7_kml  +  fzk8(k)*pk8_kml 
+  .  .  . 

fzklO(k)*pklO_kml  +  fzkll(k)*pkll_kml  +  fzkl2(k)*pkl2_kml  +  ... 
fzkl3(k)*pkl3_kml  +  fzkl4(k)*pkl4_kml  +  fzk9(k)*pk9_kml) ; 
if  pk9(k)<min,  pk9(k)=min;  end; 

pklO(k)=fzklO(k)*pklO_kml/. . . 

(f zkl (k) *pkl_kml  +  fzk2(k)*pk2_kml  +  f zk3(k) *pk3_kml  +  fzk4(k)*pk4_kml 
+  ... 

f zk5 (k) *pk5_kml  +  fzk6(k)*pk6_kml  +  f zk7(k)*pk7_kml  +  fzk8(k)*pk8_kml 
+  .  .  . 

fzk9(k)*pk9_kml  +  fzkll (k)*pkll_kml  +  fzkl2(k)*pkl2_kml  +  ... 
fzkl3(k)*pkl3_kml  +  fzkl4(k) *pkl4_kml  +  fzklO(k)*pklO_kml) ; 
if  pklO(k)<min,  pklO(k)=min;  end; 

pkll(k)=fzkll(k)*pkll_kml/. . . 

(f zkl (k) *pkl_kml  +  fzk2(k)*pk2_kml  +  fzk3(k)*pk3_kml  +  fzk4(k)*pk4_kml 
+  . . . 

f zk5 (k) *pk5_kml  +  fzk6(k)*pk6_kml  +  fzk7(k)*pk7_kml  +  fzk8(k)*pk8_kml 
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fzk9(k)*pk9_kml  +  fzklO(k)*pklO_kml  +  fzki2(k)*pkl2_kml  +  ... 
fzkl3(k)*pkl3_kml  +  fzkl4(k)*pkl4_kml  +  fzkll(k)*pkll_kml) ; 
if  pkll(k)<min,  pkll(k)=min;  end; 

pkl2(k)=fzkl2(k)*pkl2_kml/. . . 

(fzkl(k)*pkl_kml  +  fzk2(k) *pk2_kml  +  f zk3(k)*pk3_kml  +  f zk4(k) *pk4_kml 
+  .  .  . 

fzk5(k)*pk5_kml  +  fzk6(k)*pk6_kml  +  fzk7(k)*pk7_kml  +  f zk8(k) *pk8_kml 
+  .  .  . 

fzk9(k)*pk9_kml  +  fzklO(k)*p'klO_kml  +  fzkll  (k)*pkll_kml  +  ... 
fzkl3(k)*pkl3_kml  +  fzk!4(k) *pkl4_kml  +  fzkl2(k)*pkl2_kml) ; 
if  pkl2(k)<min,  pkl2(k)=min;  end; 

pkl3(k)=fzkl3(k)*pkl3_kml/. . . 

(fzkl(k)*pkl_kml  +  fzk2(k) *pk2_kml  +  fzk3(k)*pk3_kml  +  fzk4(k)*pk4_kml 
+  ... 

fzk5(k)*pk5_kml  +  fzk6(k)*pk6_kml  +  f zk7(k)*pk7_kml  +  fzk8(k)*pk8_kml 
+  ... 

fzk9(k)*pk9_kml  +  fzklO(k)*pklO_kml  +  fzkll (k)*pkll_kml  +  ... 
fzkl2(k)*pkl2_kml  +  fzkl4(k)*pkl4_kml  +  fzkl3(k)*pkl3_kml) ; 
if  pkl3(k)<min,  pkl3(k)=min;  end; 

pkl4(k)=fzkl4(k)*pkl4_kml/. . . 

(fzkl(k)*pkl_kml  +  fzk2(k)*pk2_kml  +  fzk3(k)*pk3_kml  +  fzk4(k)*pk4_kml 
+  .  .  . 

fzk5(k)*pk5_kml  +  fzk6(k)*pk6_kml  +  f zk7 (k) *pk7_kml  +  fzk8(k)*pk8_kml 
+  ... 

f zk9 (k) *pk9_kml  +  fzklO(k)*pklO_kml  +  fzkll (k)*pkll_kml  +  ... 
fzkl2(k)*pkl2_kml  +  fzkl3(k)*pkl3_kml  +  fzkl4(k)*pkl4_kml) ; 
if  pkl4(k)<min,  pkl4(k)=min;  end; 


^calculation  for  the  rest  of  the  samples 


for  k  =  2 : km ax 


i.  /././././.a 


fzkl(k)  =  1  /  ((2*pi)~(ra*(l/2))*det(al(: , (k*ra-(ra-l) :k*ra))) 
-0.5)  *  ... 

exp(-0.5*resl( : ,k) ’*inv(al( : , (k*ra-(ra-l) :k*ra)))  *  ... 
resl( : ,k)  ) ; 

fzk2(k)  =  1  /  ((2*pi)“(ra*(l/2))*det(a2(: , (k*ra-(ra-l) :k*ra))) 
-0.5)  *  . . . 

exp(-0.5*res2(: ,k)’*inv(a2(: ,(k*ra-(ra-l) :k*ra)))  *  ... 
res2( : ,k)  ) ; 

fzk3(k)  =  1  /  ((2*pi)~(ra*(l/2))*det(a3(: , (k*ra-(ra-l) :k*ra))) 
*0.5)  *  ... 

exp(-0 . 5*res3( : ,k) ’*inv(a3( : , (k*ra-(ra-l) :k*ra)))  *  ... 
res3( : ,k)  ) ; 
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fzk4(k)  =  1  /  ((2*pi)*(ra*(l/2))*det(a4(:  ,  (k*ra-(ra-l)  :k*ra))) 
*0.5)  *  .  .  . 

exp(-0 .5*res4( : ,k) ’*inv(a4( : , (k*ra-(ra-l) :k*ra) ))  *  ... 
res4( : ,k)  ) ; 

fzk5(k)  =  1  /  ((2*pi)~(ra*(l/2))*det(a5(: , (k*ra-(ra-l) :k*ra))) 
*0.5)  *  ... 

exp(-0.5*res5(:  ,kV*inv(a5(:  ,(k*ra-(ra-l)  :k*ra)))  *  ... 
res5( : ,k)  ) ; 

fzk6(k)  =  1  /  ( (2*pi) * (ra*(l/2) )*det (a6( : , (k*ra-(ra-l) :k*ra))) 
*0.5)  *  ... 

exp(-0 . 5*res6( : ,k) ’*inv(a6( : , (k*ra-(ra-l) :k*ra)))  *  ... 
res6( : ,k)  ) ; 

fzk7(k)  =  1  /  ((2*pi)~(ra*(l/2))*det(a7(: , (k*ra-(ra-l) :k*ra))) 
*0.5)  *  .  .  . 

exp(-0 . 5*res7 ( : ,k) ’ *inv(a7 ( : , (k*ra-(ra-l) :k*ra) ))  *  ... 
res7(: ,k)  ) ; 

fzk8(k)  =  1  /  ((2*pi)*(ra*(l/2))*det(a8(: , (k*ra-(ra-l) :k*ra))) 
*0.5)  *  .  .  . 

exp(-0.5*res8(: ,k) ’ *inv(a8( : , (k*ra-(ra-l) :k*ra)))  *  ... 
res8(: ,k)  ) ; 

fzk9(k)  =  1  /  ((2*pi) ' (ra*(l/2))*det(a9( : , (k*ra-(ra-l) :k*ra) ) ) 
*0.5)  *  ... 

exp(-0.5*res9( : ,k) '*inv(a9( : , (k*ra-(ra-l) :k*ra)))  *  ... 
res9( : ,k)  ) ; 

fzklO(k)  *  1  /  ( (2*pi) * (ra*(l/2) )*det (alO( : , (k*ra-(ra-l) : k*ra) ) ) 
*0.5)  *  . .  . 

exp(-0.5*resl0(: ,k) ’ *inv(alO( : , (k*ra-(ra-l) :k*ra)))  *  ... 
reslO(:,k)  ); 

fzkll(k)  =11  ((2*pi)“(ra*(l/2))*det(all(: , (k*ra-(ra-l) :k*ra))) 
*0.5)  *  ... 

exp(-0.5*resll(: ,k) ’*inv(all(: , (k*ra-(ra-l) :k*ra)))  *  ... 
resll( : ,k)  ) ; 

fzkl2(k)  =  1  /  ((2*pi)“(ra*(l/2))*det(al2(: , (k*ra-(ra-l) :k*ra))) 
*0.5)  *  ... 

exp(-0.5*resl2(: ,k) '*inv(al2(: , (k*ra-(ra-l) :k*ra)))  *  ... 
resl2(:,k)  ); 

fzkl3(k)  =  1  /  ((2*pi)~(ra*(l/2))*det(al3(:,(k*ra-(ra-l):k*ra))) 
*0.5)  *  .  . . 

exp(-0.5*resl3(: ,k) ,*inv(al3(: , (k*ra-(ra-l) :k*ra)))  *  ... 
resl3( : ,k)  ) ; 
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fzkl4(k)  *  1  /  ((2*pi)~(ra*(l/2))*det(al4(: ,(k*ra-(ra-l) :k*ra))) 
‘0.5)  *  . . . 

exp(-0.5*resl4(: ,k) ’*inv(al4( : , (k*ra-(ra-l) :k*ra)))  *  ... 
res 14 ( :  ,k)  ) ; 


pkl(k)=fzkl(k)*pkl(k-l)/. . . 

(fzk2(k)*pk2(k-l)  +  fzk3(k)*pk3(k-l)  +  fzk4(k)*pk4(k-l)  +  fzk5(k)* 
pk5(k-l)  +  ... 

fzk6(k)*pk6(k-l)  +  fzk7(k)*pk7(k-l)  +  fzk8(k)*pk8(k-l)  +  fzk9(k)* 
pk9(k-l)  +  ... 

fzklO(k)*pklO(k-l)  +  fzkll(k)*pkll(k-l)  +  fzkl2(k)*pkl2(k-l)  +  .. 
fzkl3(k)*pkl3(k-l)  +  fzkl4(k)*pkl4(k-l)  +  fzkl(k)*pkl(k-l)) ; 
if  pkl(k)<min,  pkl(k)=min;  end; 


nk2  (k )  =f zk2  Ck)  *t>k2  fk- 1 )  / 

(f zkl (k) *pkl (k- 1 )  +  fzk3(k) *pk3(k-l)  +  fzk4(k)*pk4(k-l)  +  fzk5(k)* 
pk5(k-l)  +  . . . 

fzk6(k)*pk6(k-l)  +  fzk7(k)*pk7(k-l)  +  fzk8(k) *pk8(k-l)  +  fzk9(k)* 
pk9(k-l)  +  . . . 

fzklO(k)*pklO(k-l)  +  fzkll(k)*pkll(k-l)  +  fzkl2(k)*pkl2(k-l)  +  ... 
fzkl3(k)*pkl3(k-l)  +  fzkl4(k)*pkl4(k-l)  +  fzk2(k)*pk2(k-l)) ; 
if  pk2(k)<min,  pk2(k)=min;  end; 


pk3(k)=fzk3(k)*pk3(k-l)/. . . 

(f zkl (k) *pkl (k- 1 )  +  f zk2 (k) *pk2 (k- 1 )  +  fzk4(k)*pk4(k-l)  +  fzk5(k)* 
pk5 (k-1)  +  ... 

fzk6(k)*pk6(k-l)  +  fzk7(k)*pk7(k-l)  +  fzk8(k)*pk8(k-l)  +  fzk9(k)* 
pk9(k-l)  +  . . . 

fzklO(k)*pklO(k-l)  +  fzkll(k)*pkll(k-l)  +  fzkl2(k)*pkl2(k-l)  +  . . 
fzkl3(k)*pkl3(k-l)  +  fzkl4(k)*pkl4(k-l)  +  fzk3(k)*pk3(k-l) ) ; 
if  pk3(k)<min,  pk3(k)=min;  end; 


pk4(k)=fzk4(k)*pk4(k-l)/ . . . 

(fzkl(k)*pkl(k-l)  +  fzk2(k)*pk2(k-l)  +  fzk3(k)*pk3(k-l)  +  fzk5(k)* 
pk5(k-l)  +  .  .  . 

fzk6(k)*pk6(k-l)  +  fzk7(k)*pk7(k-l)  +  fzk8(k)*pk8(k-l)  +  fzk9(k)* 
pk9(k-l)  +  ... 

f zklO (k) *pklO (k-1)  +  f zkl 1 (k) *pkl 1 (k- 1 )  +  fzkl2(k)*pkl2(k-l)  +  ... 
fzkl3(k)*pkl3(k-l)  +  fzkl4(k)*pkl4(k-l)  +  fzk4(k)*pk4(k-l)) ; 
if  pk4(k)<ain,  pk4(k)=min;  end; 


pk5(k)=fzk5(k)*pk5(k-l)/. . . 

(fzkl(k)*pkl(k-l)  +  fzk2(k)*pk2(k-l)  +  fzk3(k)*pk3(k-l)  +  fzk4(k)* 
pk4(k-l)  +  ... 

fzk6(k)*pk6(k-l)  +  fzk7(k)*pk7(k-l)  +  fzk8(k)*pk8(k-l)  +  fzk9(k)* 
pk9 (k-1)  +  . . . 
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f zklO (k) *pklO (k- 1 )  +  f zkl 1 (k) *pkl 1 (k- 1 )  +  fzkl2(k)*pkl2(k-l)  + 
fzkl3(k)*pkl3(k-l)  +  fzkl4(k)*pkl4(k-i)  +  fzk5(k)*pk5(k-l)) ; 
if  pk5(k)<min,  pk5(k)=min;  end; 


pk6  (k)  -f  zk6  (k)  *pk6  (k-  a  )  /  .  .  . 

(fzkl(k)*pkl(k-l)  r  fzk2(k)*pk2(k-l)  +  fzk3(k)*pk3(k-l)  +  fzk4(k)* 
pk4(k~l)  +  .  .  . 

fzk5(k)*ok5(k-l)  +  fzk7(k)*pk7(k-l)  +  fzk8(k)*pk8(k-l)  +  fzk9(k)* 
pk9(k-l)  +  .  .  . 

fzkl(Kk)*pklO(k-l)  +  fzkll(k)*pkll(k-l)  +  fzkl2(k)*pkl2(k-l)  +  .. 
fzki3(k)*pkl3(k-l)  +  fzkl4(k)*pkl4(k-l)  +  fzk6(k)*pk6(k-l)) ; 
if  pk6(k)<min,  pk6(k)=min;  end; 


pk7 (k) -f zk7 (k) *pk7 (k- 1 ) / . . . 

(f zkl (k) *pkl (k-1 )  +  f zk2 (k) *pk2 (k- 1 )  +  fzk3(k)*pk3(k-l)  +  fzk4(k)* 
pk4(k-l)  +  . . . 

fzk5(k)*pk5(k-l)  +  fzk6(k)*pk6(k-l)  +  fzk8(k)*pk8(k-l)  +  fzk9(k)* 
pk9(k-l)  +  .  .  . 

fzklO(k)*pklO(k-l)  +  fzkll(k)*pkll(k-l)  +  fzkl2(k)*pkl2(k-l)  +  .. 
f zkl3 (k) *pkl3 (k- 1 )  +  fzkl4(k)*pkl4(k-l)  +  fzk7(k)*pk7(k-l)) ; 
if  pk7(k)<min,  pk7(k)=min;  end; 


pk8(k)=fzk8(k)*pk8(k-l)/. . . 

(fzkl(k)*pkl(k-l)  +  f zk2 (k) *pk2 (k- 1 )  +  fzk3(k)*pk3(k-l)  +  fzk4(k)* 
pk4(k-l)  +  . . . 

f zk5 (k) *pk5 (k- 1 )  +  fzk6(k)*pk6(k-l)  +  fzk7(k)*pk7(k-l)  +  fzk9(k)* 
pk9(k-l)  +  . . . 

fzklO(k)*pklO(k-l)  +  f zkl 1 (k) *pk 1 1 (k- 1 )  +  fzkl2(k)*pkl2(k-l)  +  .. 
f zkl3 (k) *pkl3 (k- 1 )  +  fzkl4(k)*pkl4(k-l)  +  fzk8(k)*pk8(k-l)) ; 
if  pk8(k)<min,  pk8(k)*=min;  end; 


pk9(k)=fzk9(k)*pk9(k-l)/. . . 

(f zkl (k) *pkl (k-1)  +  f zk2 (k) *pk2 (k- 1 )  +  f zk3(k)*pk3(k-l)  +  fzk4(k)* 
pk4(k-l)  +  . . . 

f zk5 (k) *pk5 (k- 1 )  +  fzk6(k)*pk6(k-l)  +  fzk7(k)*pk7(k-l)  +  fzk8(k)* 
pk8(k-l)  +  . . . 

fzklO(k)*pklO(k-l)  +  f zkl 1 (k) *pkl  1 (k- 1 )  +  fzkl2(k)*pkl2(k-l)  +  .. 
f zkl3 (k) *pkl3 (k-1)  +  f zkl4(k)*pkl4(k-l)  +  fzk9(k)*pk9(k-l)) ; 
if  pk9(k)<min,  pk9(k)=min;  end; 


pklO(k)=fzklO(k)*pklO(k-l)/ . . . 

(f zkl (k) *pkl (k-1)  +  f zk2 (k) *pk2 (k- 1 )  +  fzk3(k)*pk3(k-l)  +  fzk4(k)* 
pk4(k-l)  +  . . . 

f  zk5 (k) *pk5 (k- 1 )  +  fzk6(k)*pk6(k-l)  +  fzk7(k)*pk7(k-l)  +  fzk8(k)* 
pk8(k-l)  +  . . . 

f  zk9 (k) *pk9 (k-1 )  +  fzkll(k)*pkll(k-l)  +  fzkl2(k)*pk!2(k-l)  +  ... 
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fzkl3(k)*pkl3(k-l)  +  fzkl4(k)*pkl4(k-l)  +  fzklO(k)*pklO(k-l)) ; 
if  pklO(k)<min,  pklO(k)=mm;  end; 


pkll(k)=fzkll(k)*pkll(k-l)/ . .  . 

(fzkl(k)*pkl(k-l)  +  f zk2 (k) *pk2 (k- 1 )  +  f zk3(k) *pk3(k-l)  +  fzk4(k)* 
pk4(k-l)  +  . . . 

fzk5(k)*pk5(k-l)  +  fzk6(k)*pk6(k-l)  +  fzk7(k)*pk7(k-l)  +  fzk8(k)* 
pk8(k-l)  +  . . . 

fzk9(k)*pk9(k-l)  +  fzklO(k)*pklO(k-l)  +  fzkl2(k)*pkl2(k-l)  +  . . . 
fzkl3(k)*pkl3(k-l)  +  fzkl4(k)*pkl4(k-l)  +  fzkll(k)*pkll(k-l)) ; 
if  pkll(k)<min,  pkll(k)=mm;  end; 


pkl2(k)=fzkl2(k)*pkl2(k-l)/ . . . 

(fzkl.(k)*pkl(k-l)  +  fzk2(k)*pk2(k-l)  +  fzk3(k)*pk3(k-l)  +  fzk4(k)* 
pk4(k-l)  +  . . . 

fzk5(k)*pk5(k-l)  +  fzk6(k)*pk6(k-l)  +  fzk7(k)*pk7(k-l)  +  fzk8(k)* 
pk8(k-l)  +  ... 

fzk9(k)*pk9(k-l)  +  fzklO(k)*pklO(k-l)  +  fzkll (k)*pkll(k-l)  +  ... 
fzkl3(k)*pkl3(k-l)  +  fzkl4(k)*pkl4(k-l)  +  fzkl2(k)*pkl2(k-l) ) ; 
if  pkl2(k)<mm,  pkl2(k)=min;  end; 


pkl3(k)=fzkl3(k)*pkl3(k-l)/ . . . 

(fzkl(k)*pkl(k-l)  +  fzk2(k)*pk2(k-l)  +  fzk3(k)*pk3(k-l)  +  fzk4(k)* 
pk4(k-l)  +  . . . 

f zk5 (k) *pk5 (k- 1 )  +  fzk6(k)*pk6(k-l)  +  fzk7(k)*pk7(k-l)  +  fzk8(k)* 
pk8(k-l)  +  . . . 

f zk9 (k) *pk9 (k- 1 )  +  fzklO(k)*pklO(k-l)  +  fzkll(k)*pkll(k-l)  +  ... 
f zkl2 (k) *pkl2 (k- 1)  +  fzkl4(k)*pkl4(k-l)  +  fzkl3(k)*pkl3(k-l) ) ; 
if  pk!3(k)<min,  pkl3(k)=min;  end; 


pkl4(k)=fzkl4(k)*pkl4(k-l)/ . . . 

(fzkl(k)*pkl(k-l)  +  fzk2(k)*pk2(k-l)  +  fzk3(k)*pk3(k-l)  +  fzk4(k)* 
pk4(k-l)  +  . . . 

fzk5(k)*pk5(k-l)  +  fzk6(k)*pk6(k-l)  +  fzk7(k)*pk7(k-l)  +  fzk8(k)* 
pk8(k-l)  +  . . . 

fzk9(k)*pk9(k-l)  +  fzklO(k)*pklO(k-l)  +  fzkll(k)*pkll(k-l)  +  ... 
f zkl2 (k) *pkl2 (k- 1 )  +  fzkl3(k)*pkl3(k-l)  +  fzkl4(k)*pk!4(k-l)) ; 
if  pk!4(k)<min,  pk!4(k)=min;  end; 


end;  */,  index  k 


p!ot(l :k,pkl) 

title( 'Probability  of  no  error') 
xlabel( 'observation  index  (k)') 
ylabel ( ’ Probability ' ) 
pause 
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plotCl :k,pk2) 

titleC 'Probability  of  elevator  failure') 
xlabel( 'observation  index  (k)') 
ylabel ( 'Probability ' ) 
pause 

plotCl :k,pk3) 

titleC 'Probability  of  aileron  failure') 
xlabel(' observation  index  (k)') 
ylabel ( ' Probability ' ) 
pause 

plot(l :k,pk4) 

title ( 'Probability  of  rudder  failure') 
xlabelC 'observation  index  (k)  ' ) 
ylabel ( ’ Probability ' ) 
pause 

plot (1 :k,pk5) 

title (’Probability  of  thrust  actuator  failure') 
xlabel( ’ observation  index  (k)  ’ ) 
ylabel ( ' Probability ' ) 
pause 

plot(l:k,pk6) 

title ( 'Probability  of  u,  forward  velocity  sensor  failure') 
xlabelC’ observation  index  (k)') 
ylabel ( ' Probability ’ ) 
pause 


plot(l :k,pk7) 

t it le( 'Probability  of  v,  lateral  velocity  sensor  failure’) 
xlabel( ’ observation  index  (k)’) 
ylabel ( ’ Probability ' ) 
pause 

plotCl :k,pk8) 

title ( 'Probability  of  w,  vertical  velocity  sensor  failure’) 
xlabelC 'observation  index  (k)') 
ylabel ( 'Probability' ) 
pause 

plotCl :k,pk9) 

titled  Probability  of  p,  roll  rate  sensor  failure') 
xlabelC 'observation  index  (k) ’ ) 
ylabel ( ' Probability ’ ) 
pause 

plotCl :k,pklO) 


title( ’Probability  of  q,  pitch  rate  sensor  failure’) 
xlabel(’ observation  index  (k)’) 
ylabel ( ’Probability’ ) 
pause 

plot (l rk.pkll) 

title( 'Probability  of  r,  yaw  rate  sensor  failure') 
xlabel(’ observation  index  (k) ' ) 
ylabel ( ’ Probability ' ) 
pause 

plot(l:k,pkl2) 

title( ’Probability  of  phi,  roll  sensor  failure’) 
xlabel( ’ observation  index  (k) ’ ) 
ylabel ( ’ Probability ’ ) 
pause 

plot (1 :k,pkl3) 

title( ’Probability  of  theta,  angle  of  attack  sensor  failure') 
xlabelC’ observation  index  (k)’) 
ylabel ( ’ Probability ’ ) 
pause 

plot(l :k,pkl4) 

titleC ’Probability  of  psi,  heading  sensor  failure') 
xlabel ( ’ observat ion  index  (k) ’ ) 
ylabel ( ’ Probability ’ ) 
pause 
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